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

将ICM42688改为硬件SPI通讯。

Browse Source
This commit is contained in:
lijie
2024-02-22 10:16:24 +08:00
parent 08cbf4ba74
commit 7749d1d7b7
19 changed files with 1391 additions and 468 deletions
Download Patch File Download Diff File Expand all files Collapse all files

38
APP/LED_RGB.c Normal file
View File

@ -0,0 +1,38 @@
//
// Created by hu123456 on 2024/2/22.
//
#include "LED_RGB.h"
void LED_RGB_Init()
{
HAL_TIM_Base_Start(&htim4); //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
HAL_TIM_PWM_Start(&htim4,TIM_CHANNEL_1);
HAL_TIM_PWM_Start(&htim4,TIM_CHANNEL_2);
HAL_TIM_PWM_Start(&htim4,TIM_CHANNEL_3);
}
void Set_RGB_color(enum RGB_Color color)
{
switch (color) {
case black:
__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_1,0);
__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_2,0);
__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_3,0);
break;
case red:
__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_1,200);
__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_2,0);
__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_3,0);
break;
case green:
__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_1,0);
__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_2,200);
__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_3,0);
break;
case blue:
__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_1,0);
__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_2,0);
__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_3,200);
break;
}
}

20
APP/LED_RGB.h Normal file
View File

@ -0,0 +1,20 @@
//
// Created by hu123456 on 2024/2/22.
//
#ifndef FOC_N_LED_RGB_H
#define FOC_N_LED_RGB_H
#include "mymain.h"
enum RGB_Color
{
black,
red,
green,
blue
};
void LED_RGB_Init();
void Set_RGB_color(enum RGB_Color color);
#endif //FOC_N_LED_RGB_H

113
APP/Usart1.c Normal file
View File

@ -0,0 +1,113 @@
//
// Created by hu123456 on 2024/2/20.
//
#include "Usart1.h"
#include "stdio.h"
#include <stdlib.h>
#if USART_EN_RX
uint16_t g_usart_rx_sta = 0;
uint8_t g_usart_rx_buf[USART_REC_LEN];
uint8_t g_rx_buffer[RXBUFFERSIZE];
void set_pid();
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
if(huart->Instance == USART1)
{
if((g_usart_rx_sta & 0x8000) == 0)
{
if(g_usart_rx_sta & 0x4000)
{
if(g_rx_buffer[0] != 0x0a)
{
g_usart_rx_sta = 0;
}
else
{
g_usart_rx_sta |= 0x8000;
}
}
else
{
if(g_rx_buffer[0] == 0x0d)
{
g_usart_rx_sta |= 0x4000;
}
else
{
g_usart_rx_buf[g_usart_rx_sta & 0X3FFF] = g_rx_buffer[0] ;
g_usart_rx_sta++;
if(g_usart_rx_sta > (USART_REC_LEN - 1))
{
g_usart_rx_sta = 0;
}
}
}
}
HAL_UART_Receive_IT(&huart1, (uint8_t *)g_rx_buffer, RXBUFFERSIZE);
}
}
#endif
//xzp1.16
void commander_run(void)
{
if((g_usart_rx_sta&0x8000)!=0)
{
switch(g_usart_rx_buf[0])
{
case 'H':
printf("Hello World!\r\n");
break;
case 'x': //T6.28
set_pid();
break;
case 'y':
set_pid();
break;
}
g_usart_rx_sta=0;
}
}
void set_pid()
{
float a =0;
if(g_usart_rx_buf[1] == 'z')
{
switch (g_usart_rx_buf[2])
{
case 'p':
a = atof((const char *) (g_usart_rx_buf + 3));
printf("xzp:%f\r\n",a);
break;
case 'i':
FOCStruct_X.ZT_KI = atof((const char *) (g_usart_rx_buf + 3));
printf("xzi:%f\r\n",FOCStruct_X.ZT_KP);
break;
case 'd':
FOCStruct_X.ZT_KD = atof((const char *) (g_usart_rx_buf + 3));
printf("xzd:%f\r\n",FOCStruct_X.ZT_KP);
break;
}
}
else
{
switch (g_usart_rx_buf[2])
{
case 'p':
FOCStruct_X.V_KP = atof((const char *) (g_usart_rx_buf + 3));
printf("xvp:%f\r\n",FOCStruct_X.ZT_KP);
break;
case 'i':
FOCStruct_X.V_KI = atof((const char *) (g_usart_rx_buf + 3));
printf("xvi:%f\r\n",FOCStruct_X.ZT_KP);
break;
case 'd':
FOCStruct_X.V_KD = atof((const char *) (g_usart_rx_buf + 3));
printf("xvd:%f\r\n",FOCStruct_X.ZT_KP);
break;
}
}
}

20
APP/Usart1.h Normal file
View File

@ -0,0 +1,20 @@
//
// Created by hu123456 on 2024/2/20.
//
#ifndef FOC_N_USART1_H
#define FOC_N_USART1_H
#define USART_REC_LEN 200
#define USART_EN_RX 1
#define RXBUFFERSIZE 1
#include "mymain.h"
extern uint8_t g_usart_rx_buf[USART_REC_LEN];
extern uint16_t g_usart_rx_sta;
extern uint8_t g_rx_buffer[RXBUFFERSIZE];
void commander_run(void);
#endif //FOC_N_USART1_H

28
APP/foc.c
View File

@ -17,12 +17,12 @@ FOCTypeDef FOCStruct_Y={0,0,0,0,0,0,0,0,0,0,0,0,0,0};
void PID_Iint(void) void PID_Iint(void)
{ {
FOCStruct_X.ZT_KP = 2; FOCStruct_X.ZT_KP = 4;
FOCStruct_X.ZT_KI = 0; FOCStruct_X.ZT_KI = 0;
FOCStruct_X.ZT_KD = 0.2; FOCStruct_X.ZT_KD = 0.5;
FOCStruct_X.V_KP = 0.1; FOCStruct_X.V_KP = 0.1;
FOCStruct_X.V_KI = 1.5; FOCStruct_X.V_KI = 2;
FOCStruct_X.V_KD = 0; FOCStruct_X.V_KD = 0;
//2024/1/2 Y<><59>Ч<EFBFBD><D0A7><EFBFBD><EFBFBD><EFBFBD><EFBFBD> //2024/1/2 Y<><59>Ч<EFBFBD><D0A7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
FOCStruct_Y.ZT_KP = 2; FOCStruct_Y.ZT_KP = 2;
@ -33,6 +33,22 @@ void PID_Iint(void)
FOCStruct_Y.V_KI = 1; FOCStruct_Y.V_KI = 1;
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> ////2024/1/23 xY<78><59>Ч<EFBFBD><D0A7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// FOCStruct_X.ZT_KP = 2; // FOCStruct_X.ZT_KP = 2;
@ -50,7 +66,6 @@ void PID_Iint(void)
// FOCStruct_Y.V_KP = 0.1; // FOCStruct_Y.V_KP = 0.1;
// FOCStruct_Y.V_KI = 1; // FOCStruct_Y.V_KI = 1;
// FOCStruct_Y.V_KD = 0; // 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)
{ {
@ -67,7 +82,8 @@ void setPhaseVoltage(unsigned char motor,float Uq, float Ud, double angle_el)
angle_el = _normalizeAngle(angle_el + atan2(Uq, Ud)); angle_el = _normalizeAngle(angle_el + atan2(Uq, Ud));
} }
else else
{// only Uq available - no need for atan2 and sqrt {
// only Uq available - no need for atan2 and sqrt
Uout = Uq / Voltage_PowerSupply; Uout = Uq / Voltage_PowerSupply;
// angle normalisation in between 0 and 2pi // angle normalisation in between 0 and 2pi
// only necessary if using _sin and _cos - approximation functions // only necessary if using _sin and _cos - approximation functions
@ -160,7 +176,7 @@ void speed_closed_loop(unsigned motor,FOCTypeDef *FOCStruct,float V_Target)
float V_Pid_I = FOCStruct->V_Pid_IPrev + FOCStruct->V_KI*Ts*0.5*(V_Error + FOCStruct->V_Error_Prev); float V_Pid_I = FOCStruct->V_Pid_IPrev + FOCStruct->V_KI*Ts*0.5*(V_Error + FOCStruct->V_Error_Prev);
V_Pid_I = _constrain(V_Pid_I, -Voltage_Limit, Voltage_Limit); V_Pid_I = _constrain(V_Pid_I, -Voltage_Limit, Voltage_Limit);
float V_Out = V_Pid_P+V_Pid_I; float V_Out = V_Pid_P + V_Pid_I;
V_Out = _constrain(V_Out, -Voltage_Limit, Voltage_Limit); V_Out = _constrain(V_Out, -Voltage_Limit, Voltage_Limit);
float V_Acc_Now = (V_Out - FOCStruct->V_Out_Prev)/Ts; float V_Acc_Now = (V_Out - FOCStruct->V_Out_Prev)/Ts;

21
APP/mymain.c
View File

@ -6,6 +6,7 @@
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_Delay(2000); HAL_Delay(2000);
led_init(); led_init();
tle5012b_init(); tle5012b_init();
@ -17,13 +18,13 @@ void mymain()
float x = 0,y=0; float x = 0,y=0;
unsigned int i =0; unsigned int i =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<3300*3) // while(i<3300*3)
{ // {
Angel_closed_loop(motorx,&FOCStruct_X,-0.2); // Angel_closed_loop(motorx,&FOCStruct_X,-0.2);
Angel_closed_loop(motory,&FOCStruct_Y,2.3); // Angel_closed_loop(motory,&FOCStruct_Y,2.3);
i++; // i++;
} // }
i=0; // i=0;
LED_ON; LED_ON;
while(1) while(1)
{ {
@ -33,6 +34,7 @@ void mymain()
y = Angle.roll ; y = Angle.roll ;
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();
// x = getAngle(motorx); // x = getAngle(motorx);
// y = getAngle(motory); // y = getAngle(motory);
// printf("%fY%f\n",x,y); // printf("%fY%f\n",x,y);
@ -45,7 +47,10 @@ void mymain()
// } // }
// i++; // i++;
// //
Data_send(x,y); // Data_send(x,y);
// HAL_Delay(3); // HAL_Delay(3);
} }
} }

2
APP/mymain.h
View File

@ -28,6 +28,8 @@
#include "alldata.h" #include "alldata.h"
#include "usart_ano.h" #include "usart_ano.h"
#include "LED.h" #include "LED.h"
#include "Usart1.h"
#include "LED_RGB.h"
void mymain(); void mymain();
#endif //FOC_N_MYMAIN_H #endif //FOC_N_MYMAIN_H

3
Core/Inc/spi.h
View File

@ -34,11 +34,14 @@ extern "C" {
extern SPI_HandleTypeDef hspi1; extern SPI_HandleTypeDef hspi1;
extern SPI_HandleTypeDef hspi2;
/* USER CODE BEGIN Private defines */ /* USER CODE BEGIN Private defines */
/* USER CODE END Private defines */ /* USER CODE END Private defines */
void MX_SPI1_Init(void); void MX_SPI1_Init(void);
void MX_SPI2_Init(void);
/* USER CODE BEGIN Prototypes */ /* USER CODE BEGIN Prototypes */

1
Core/Inc/stm32h7xx_it.h
View File

@ -55,6 +55,7 @@ void SVC_Handler(void);
void DebugMon_Handler(void); void DebugMon_Handler(void);
void PendSV_Handler(void); void PendSV_Handler(void);
void SysTick_Handler(void); void SysTick_Handler(void);
void USART1_IRQHandler(void);
/* USER CODE BEGIN EFP */ /* USER CODE BEGIN EFP */
/* USER CODE END EFP */ /* USER CODE END EFP */

3
Core/Inc/tim.h
View File

@ -36,6 +36,8 @@ extern TIM_HandleTypeDef htim2;
extern TIM_HandleTypeDef htim3; extern TIM_HandleTypeDef htim3;
extern TIM_HandleTypeDef htim4;
extern TIM_HandleTypeDef htim5; extern TIM_HandleTypeDef htim5;
/* USER CODE BEGIN Private defines */ /* USER CODE BEGIN Private defines */
@ -44,6 +46,7 @@ extern TIM_HandleTypeDef htim5;
void MX_TIM2_Init(void); void MX_TIM2_Init(void);
void MX_TIM3_Init(void); void MX_TIM3_Init(void);
void MX_TIM4_Init(void);
void MX_TIM5_Init(void); void MX_TIM5_Init(void);
void HAL_TIM_MspPostInit(TIM_HandleTypeDef *htim); void HAL_TIM_MspPostInit(TIM_HandleTypeDef *htim);

2
Core/Src/gpio.c
View File

@ -42,6 +42,8 @@ void MX_GPIO_Init(void)
/* GPIO Ports Clock Enable */ /* GPIO Ports Clock Enable */
__HAL_RCC_GPIOH_CLK_ENABLE(); __HAL_RCC_GPIOH_CLK_ENABLE();
__HAL_RCC_GPIOA_CLK_ENABLE(); __HAL_RCC_GPIOA_CLK_ENABLE();
__HAL_RCC_GPIOB_CLK_ENABLE();
__HAL_RCC_GPIOD_CLK_ENABLE();
__HAL_RCC_GPIOC_CLK_ENABLE(); __HAL_RCC_GPIOC_CLK_ENABLE();
} }

6
Core/Src/main.c
View File

@ -92,16 +92,18 @@ int main(void)
MX_TIM5_Init(); MX_TIM5_Init();
MX_USART1_UART_Init(); MX_USART1_UART_Init();
MX_SPI1_Init(); MX_SPI1_Init();
MX_SPI2_Init();
MX_TIM4_Init();
/* USER CODE BEGIN 2 */ /* USER CODE BEGIN 2 */
/* USER CODE END 2 */ /* USER CODE END 2 */
mymain();
/* Infinite loop */ /* Infinite loop */
/* USER CODE BEGIN WHILE */ /* USER CODE BEGIN WHILE */
while (1) while (1)
{ {
/* USER CODE END WHILE */ /* USER CODE END WHILE */
mymain();
/* USER CODE BEGIN 3 */ /* USER CODE BEGIN 3 */
} }
/* USER CODE END 3 */ /* USER CODE END 3 */

97
Core/Src/spi.c
View File

@ -25,6 +25,7 @@
/* USER CODE END 0 */ /* USER CODE END 0 */
SPI_HandleTypeDef hspi1; SPI_HandleTypeDef hspi1;
SPI_HandleTypeDef hspi2;
/* SPI1 init function */ /* SPI1 init function */
void MX_SPI1_Init(void) void MX_SPI1_Init(void)
@ -67,6 +68,48 @@ void MX_SPI1_Init(void)
/* USER CODE END SPI1_Init 2 */ /* USER CODE END SPI1_Init 2 */
}
/* SPI2 init function */
void MX_SPI2_Init(void)
{
/* USER CODE BEGIN SPI2_Init 0 */
/* USER CODE END SPI2_Init 0 */
/* USER CODE BEGIN SPI2_Init 1 */
/* USER CODE END SPI2_Init 1 */
hspi2.Instance = SPI2;
hspi2.Init.Mode = SPI_MODE_MASTER;
hspi2.Init.Direction = SPI_DIRECTION_2LINES;
hspi2.Init.DataSize = SPI_DATASIZE_8BIT;
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.FirstBit = SPI_FIRSTBIT_MSB;
hspi2.Init.TIMode = SPI_TIMODE_DISABLE;
hspi2.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
hspi2.Init.CRCPolynomial = 0x0;
hspi2.Init.NSSPMode = SPI_NSS_PULSE_ENABLE;
hspi2.Init.NSSPolarity = SPI_NSS_POLARITY_HIGH;
hspi2.Init.FifoThreshold = SPI_FIFO_THRESHOLD_01DATA;
hspi2.Init.TxCRCInitializationPattern = SPI_CRC_INITIALIZATION_ALL_ZERO_PATTERN;
hspi2.Init.RxCRCInitializationPattern = SPI_CRC_INITIALIZATION_ALL_ZERO_PATTERN;
hspi2.Init.MasterSSIdleness = SPI_MASTER_SS_IDLENESS_00CYCLE;
hspi2.Init.MasterInterDataIdleness = SPI_MASTER_INTERDATA_IDLENESS_00CYCLE;
hspi2.Init.MasterReceiverAutoSusp = SPI_MASTER_RX_AUTOSUSP_DISABLE;
hspi2.Init.MasterKeepIOState = SPI_MASTER_KEEP_IO_STATE_DISABLE;
hspi2.Init.IOSwap = SPI_IO_SWAP_DISABLE;
if (HAL_SPI_Init(&hspi2) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN SPI2_Init 2 */
/* USER CODE END SPI2_Init 2 */
} }
void HAL_SPI_MspInit(SPI_HandleTypeDef* spiHandle) void HAL_SPI_MspInit(SPI_HandleTypeDef* spiHandle)
@ -109,6 +152,41 @@ void HAL_SPI_MspInit(SPI_HandleTypeDef* spiHandle)
/* USER CODE END SPI1_MspInit 1 */ /* USER CODE END SPI1_MspInit 1 */
} }
else if(spiHandle->Instance==SPI2)
{
/* USER CODE BEGIN SPI2_MspInit 0 */
/* USER CODE END SPI2_MspInit 0 */
/** Initializes the peripherals clock
*/
PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_SPI2;
PeriphClkInitStruct.Spi123ClockSelection = RCC_SPI123CLKSOURCE_PLL;
if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK)
{
Error_Handler();
}
/* SPI2 clock enable */
__HAL_RCC_SPI2_CLK_ENABLE();
__HAL_RCC_GPIOB_CLK_ENABLE();
/**SPI2 GPIO Configuration
PB13 ------> SPI2_SCK
PB14 ------> SPI2_MISO
PB15 ------> SPI2_MOSI
*/
GPIO_InitStruct.Pin = GPIO_PIN_13|GPIO_PIN_14|GPIO_PIN_15;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
GPIO_InitStruct.Alternate = GPIO_AF5_SPI2;
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
/* USER CODE BEGIN SPI2_MspInit 1 */
/* USER CODE END SPI2_MspInit 1 */
}
} }
void HAL_SPI_MspDeInit(SPI_HandleTypeDef* spiHandle) void HAL_SPI_MspDeInit(SPI_HandleTypeDef* spiHandle)
@ -133,6 +211,25 @@ void HAL_SPI_MspDeInit(SPI_HandleTypeDef* spiHandle)
/* USER CODE END SPI1_MspDeInit 1 */ /* USER CODE END SPI1_MspDeInit 1 */
} }
else if(spiHandle->Instance==SPI2)
{
/* USER CODE BEGIN SPI2_MspDeInit 0 */
/* USER CODE END SPI2_MspDeInit 0 */
/* Peripheral clock disable */
__HAL_RCC_SPI2_CLK_DISABLE();
/**SPI2 GPIO Configuration
PB13 ------> SPI2_SCK
PB14 ------> SPI2_MISO
PB15 ------> SPI2_MOSI
*/
HAL_GPIO_DeInit(GPIOB, GPIO_PIN_13|GPIO_PIN_14|GPIO_PIN_15);
/* USER CODE BEGIN SPI2_MspDeInit 1 */
/* USER CODE END SPI2_MspDeInit 1 */
}
} }
/* USER CODE BEGIN 1 */ /* USER CODE BEGIN 1 */

16
Core/Src/stm32h7xx_it.c
View File

@ -55,7 +55,7 @@
/* USER CODE END 0 */ /* USER CODE END 0 */
/* External variables --------------------------------------------------------*/ /* External variables --------------------------------------------------------*/
extern UART_HandleTypeDef huart1;
/* USER CODE BEGIN EV */ /* USER CODE BEGIN EV */
/* USER CODE END EV */ /* USER CODE END EV */
@ -198,6 +198,20 @@ void SysTick_Handler(void)
/* please refer to the startup file (startup_stm32h7xx.s). */ /* please refer to the startup file (startup_stm32h7xx.s). */
/******************************************************************************/ /******************************************************************************/
/**
* @brief This function handles USART1 global interrupt.
*/
void USART1_IRQHandler(void)
{
/* USER CODE BEGIN USART1_IRQn 0 */
/* USER CODE END USART1_IRQn 0 */
HAL_UART_IRQHandler(&huart1);
/* USER CODE BEGIN USART1_IRQn 1 */
/* USER CODE END USART1_IRQn 1 */
}
/* USER CODE BEGIN 1 */ /* USER CODE BEGIN 1 */
/* USER CODE END 1 */ /* USER CODE END 1 */

114
Core/Src/tim.c
View File

@ -26,6 +26,7 @@
TIM_HandleTypeDef htim2; TIM_HandleTypeDef htim2;
TIM_HandleTypeDef htim3; TIM_HandleTypeDef htim3;
TIM_HandleTypeDef htim4;
TIM_HandleTypeDef htim5; TIM_HandleTypeDef htim5;
/* TIM2 init function */ /* TIM2 init function */
@ -44,9 +45,9 @@ void MX_TIM2_Init(void)
/* USER CODE END TIM2_Init 1 */ /* USER CODE END TIM2_Init 1 */
htim2.Instance = TIM2; htim2.Instance = TIM2;
htim2.Init.Prescaler = 0; htim2.Init.Prescaler = 1 - 1;
htim2.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED1; htim2.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED1;
htim2.Init.Period = 4800-1; htim2.Init.Period = 4800 - 1;
htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
htim2.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; htim2.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
if (HAL_TIM_Base_Init(&htim2) != HAL_OK) if (HAL_TIM_Base_Init(&htim2) != HAL_OK)
@ -106,7 +107,7 @@ void MX_TIM3_Init(void)
/* USER CODE END TIM3_Init 1 */ /* USER CODE END TIM3_Init 1 */
htim3.Instance = TIM3; htim3.Instance = TIM3;
htim3.Init.Prescaler = 0; htim3.Init.Prescaler = 1 - 1;
htim3.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED1; htim3.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED1;
htim3.Init.Period = 4800-1; htim3.Init.Period = 4800-1;
htim3.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; htim3.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
@ -151,6 +152,68 @@ void MX_TIM3_Init(void)
/* USER CODE END TIM3_Init 2 */ /* USER CODE END TIM3_Init 2 */
HAL_TIM_MspPostInit(&htim3); HAL_TIM_MspPostInit(&htim3);
}
/* TIM4 init function */
void MX_TIM4_Init(void)
{
/* USER CODE BEGIN TIM4_Init 0 */
/* USER CODE END TIM4_Init 0 */
TIM_ClockConfigTypeDef sClockSourceConfig = {0};
TIM_MasterConfigTypeDef sMasterConfig = {0};
TIM_OC_InitTypeDef sConfigOC = {0};
/* USER CODE BEGIN TIM4_Init 1 */
/* USER CODE END TIM4_Init 1 */
htim4.Instance = TIM4;
htim4.Init.Prescaler = 240 - 1;
htim4.Init.CounterMode = TIM_COUNTERMODE_UP;
htim4.Init.Period = 255;
htim4.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
htim4.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
if (HAL_TIM_Base_Init(&htim4) != HAL_OK)
{
Error_Handler();
}
sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
if (HAL_TIM_ConfigClockSource(&htim4, &sClockSourceConfig) != HAL_OK)
{
Error_Handler();
}
if (HAL_TIM_PWM_Init(&htim4) != HAL_OK)
{
Error_Handler();
}
sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
if (HAL_TIMEx_MasterConfigSynchronization(&htim4, &sMasterConfig) != HAL_OK)
{
Error_Handler();
}
sConfigOC.OCMode = TIM_OCMODE_PWM1;
sConfigOC.Pulse = 0;
sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
if (HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_1) != HAL_OK)
{
Error_Handler();
}
if (HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_2) != HAL_OK)
{
Error_Handler();
}
if (HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_3) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN TIM4_Init 2 */
/* USER CODE END TIM4_Init 2 */
HAL_TIM_MspPostInit(&htim4);
} }
/* TIM5 init function */ /* TIM5 init function */
void MX_TIM5_Init(void) void MX_TIM5_Init(void)
@ -218,6 +281,17 @@ void HAL_TIM_Base_MspInit(TIM_HandleTypeDef* tim_baseHandle)
/* USER CODE END TIM3_MspInit 1 */ /* USER CODE END TIM3_MspInit 1 */
} }
else if(tim_baseHandle->Instance==TIM4)
{
/* USER CODE BEGIN TIM4_MspInit 0 */
/* USER CODE END TIM4_MspInit 0 */
/* TIM4 clock enable */
__HAL_RCC_TIM4_CLK_ENABLE();
/* USER CODE BEGIN TIM4_MspInit 1 */
/* USER CODE END TIM4_MspInit 1 */
}
else if(tim_baseHandle->Instance==TIM5) else if(tim_baseHandle->Instance==TIM5)
{ {
/* USER CODE BEGIN TIM5_MspInit 0 */ /* USER CODE BEGIN TIM5_MspInit 0 */
@ -279,6 +353,29 @@ void HAL_TIM_MspPostInit(TIM_HandleTypeDef* timHandle)
/* USER CODE END TIM3_MspPostInit 1 */ /* USER CODE END TIM3_MspPostInit 1 */
} }
else if(timHandle->Instance==TIM4)
{
/* USER CODE BEGIN TIM4_MspPostInit 0 */
/* USER CODE END TIM4_MspPostInit 0 */
__HAL_RCC_GPIOD_CLK_ENABLE();
/**TIM4 GPIO Configuration
PD12 ------> TIM4_CH1
PD13 ------> TIM4_CH2
PD14 ------> TIM4_CH3
*/
GPIO_InitStruct.Pin = GPIO_PIN_12|GPIO_PIN_13|GPIO_PIN_14;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
GPIO_InitStruct.Alternate = GPIO_AF2_TIM4;
HAL_GPIO_Init(GPIOD, &GPIO_InitStruct);
/* USER CODE BEGIN TIM4_MspPostInit 1 */
/* USER CODE END TIM4_MspPostInit 1 */
}
} }
@ -307,6 +404,17 @@ void HAL_TIM_Base_MspDeInit(TIM_HandleTypeDef* tim_baseHandle)
/* USER CODE END TIM3_MspDeInit 1 */ /* USER CODE END TIM3_MspDeInit 1 */
} }
else if(tim_baseHandle->Instance==TIM4)
{
/* USER CODE BEGIN TIM4_MspDeInit 0 */
/* USER CODE END TIM4_MspDeInit 0 */
/* Peripheral clock disable */
__HAL_RCC_TIM4_CLK_DISABLE();
/* USER CODE BEGIN TIM4_MspDeInit 1 */
/* USER CODE END TIM4_MspDeInit 1 */
}
else if(tim_baseHandle->Instance==TIM5) else if(tim_baseHandle->Instance==TIM5)
{ {
/* USER CODE BEGIN TIM5_MspDeInit 0 */ /* USER CODE BEGIN TIM5_MspDeInit 0 */

6
Core/Src/usart.c
View File

@ -67,6 +67,7 @@ void MX_USART1_UART_Init(void)
} }
/* USER CODE BEGIN USART1_Init 2 */ /* USER CODE BEGIN USART1_Init 2 */
/* USER CODE END USART1_Init 2 */ /* USER CODE END USART1_Init 2 */
} }
@ -106,6 +107,9 @@ void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle)
GPIO_InitStruct.Alternate = GPIO_AF7_USART1; GPIO_InitStruct.Alternate = GPIO_AF7_USART1;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
/* USART1 interrupt Init */
HAL_NVIC_SetPriority(USART1_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(USART1_IRQn);
/* USER CODE BEGIN USART1_MspInit 1 */ /* USER CODE BEGIN USART1_MspInit 1 */
/* USER CODE END USART1_MspInit 1 */ /* USER CODE END USART1_MspInit 1 */
@ -129,6 +133,8 @@ void HAL_UART_MspDeInit(UART_HandleTypeDef* uartHandle)
*/ */
HAL_GPIO_DeInit(GPIOA, GPIO_PIN_9|GPIO_PIN_10); HAL_GPIO_DeInit(GPIOA, GPIO_PIN_9|GPIO_PIN_10);
/* USART1 interrupt Deinit */
HAL_NVIC_DisableIRQ(USART1_IRQn);
/* USER CODE BEGIN USART1_MspDeInit 1 */ /* USER CODE BEGIN USART1_MspDeInit 1 */
/* USER CODE END USART1_MspDeInit 1 */ /* USER CODE END USART1_MspDeInit 1 */

782
IMU/icm42688.c
View File

@ -3,36 +3,22 @@
#include "kalman.h" #include "kalman.h"
#include "alldata.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; static float icm42688_acc_inv = 1, icm42688_gyro_inv = 1;
void Spi_GPIO_Init() void Spi_GPIO_Init()
{ {
GPIO_InitTypeDef GPIO_InitStruct = {0}; GPIO_InitTypeDef GPIO_InitStruct = {0};
__HAL_RCC_GPIOB_CLK_ENABLE(); __HAL_RCC_GPIOB_CLK_ENABLE();
/*Configure GPIO pins : PBPin PB12 */ /*Configure GPIO pins : PBPin PB12 */
GPIO_InitStruct.Pin = GPIO_PIN_12 | GPIO_PIN_13 |GPIO_PIN_15; GPIO_InitStruct.Pin = GPIO_PIN_12;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL; GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; 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); HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
} }
void Init_ICM42688(void) void Init_ICM42688(void)
{ {
unsigned char time;
unsigned char model; unsigned char model;
Spi_GPIO_Init(); Spi_GPIO_Init();
// <20><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͺ<EFBFBD><CDBA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Լ<EFBFBD> // <20><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͺ<EFBFBD><CDBA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Լ<EFBFBD>
@ -42,68 +28,18 @@ void Init_ICM42688(void)
Read_Datas_ICM42688(ICM42688_WHO_AM_I, &model, 1); Read_Datas_ICM42688(ICM42688_WHO_AM_I, &model, 1);
if(model == 0x47) if(model == 0x47)
{ {
// ICM42688
break; break;
} }
ICM42688_DELAY_MS(10); ICM42688_DELAY_MS(10);
} }
Write_Data_ICM42688(ICM42688_PWR_MGMT0,0x00); // <20><>λ<EFBFBD>豸 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> 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> // <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); 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>ģʽ Write_Data_ICM42688(ICM42688_PWR_MGMT0, 0x0f); // <20><><EFBFBD><EFBFBD>GYRO_MODE,ACCEL_MODEΪ<45><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģʽ
ICM42688_DELAY_MS(10); 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) 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_ACCEL_CONFIG0, (afs << 5) | (aodr + 1)); // <20><>ʼ<EFBFBD><CABC>ACCEL<45><4C><EFBFBD>̺<EFBFBD><CCBA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>(p77)
@ -159,172 +95,6 @@ void Set_LowpassFilter_Range_ICM42688(enum icm42688_afs afs, enum icm42688_aodr
} }
} }
/**
*
* @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> * @brief ICM42688<38><38><EFBFBD><EFBFBD><EFBFBD><EFBFBD>д<EFBFBD><D0B4><EFBFBD><EFBFBD>
@ -337,10 +107,10 @@ static void Read_8bit_ICM42688(unsigned char *dat)
**/ **/
static void Write_Data_ICM42688(unsigned char reg, unsigned char dat) static void Write_Data_ICM42688(unsigned char reg, unsigned char dat)
{ {
sys_gpio_pin_set(GPIOB,GPIO_PIN_12,0); HAL_GPIO_WritePin(GPIOB,GPIO_PIN_12,0);
Write_8bit_ICM42688(reg); HAL_SPI_Transmit(&hspi2, &reg, 1, 0xff);
Write_8bit_ICM42688(dat); HAL_SPI_Transmit(&hspi2, &dat, 1, 0xff);
sys_gpio_pin_set(GPIOB,GPIO_PIN_12,1); HAL_GPIO_WritePin(GPIOB,GPIO_PIN_12,1);
} }
/** /**
@ -356,11 +126,17 @@ static void Write_Data_ICM42688(unsigned char reg, unsigned char dat)
**/ **/
static void Read_Datas_ICM42688(unsigned char reg, unsigned char *dat, unsigned int num) static void Read_Datas_ICM42688(unsigned char reg, unsigned char *dat, unsigned int num)
{ {
sys_gpio_pin_set(GPIOB,GPIO_PIN_12,0); uint8_t Data;
reg |= 0x80; reg |= 0x80;
Write_8bit_ICM42688(reg); HAL_GPIO_WritePin(GPIOB,GPIO_PIN_12,0);
while(num--) Read_8bit_ICM42688(dat++); HAL_SPI_Transmit(&hspi2, &reg, 1, 0xff );
sys_gpio_pin_set(GPIOB,GPIO_PIN_12,1); while(num--)
{
HAL_SPI_Receive(&hspi2,&Data, 1, 0xff);
*dat = Data;
dat++;
}
HAL_GPIO_WritePin(GPIOB,GPIO_PIN_12,1);
} }
/* <20><>ȡMPU6050<35><30><EFBFBD>ݲ<EFBFBD><DDB2><EFBFBD><EFBFBD>˲<EFBFBD> */ /* <20><>ȡMPU6050<35><30><EFBFBD>ݲ<EFBFBD><DDB2><EFBFBD><EFBFBD>˲<EFBFBD> */
//<2F><><EFBFBD><EFBFBD>ֵ:0,<2C><><EFBFBD><EFBFBD> //<2F><><EFBFBD><EFBFBD>ֵ:0,<2C><><EFBFBD><EFBFBD>
@ -373,7 +149,7 @@ uint8_t MpuGetData(void)
{ {
uint8_t i; uint8_t i;
int res; int res;
unsigned char buffer[12]; uint8_t buffer[12];
Read_Datas_ICM42688(ICM42688_ACCEL_DATA_X1,buffer,12); Read_Datas_ICM42688(ICM42688_ACCEL_DATA_X1,buffer,12);
for(i=0;i<6;i++) for(i=0;i<6;i++)
@ -399,44 +175,494 @@ uint8_t MpuGetData(void)
return 0; 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;
}
}
/////////////ģ<><C4A3>spi<70>汾
//#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));
//}
//
///**
//<2F><><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;
// }
//}
//
//
//

309
IMU/icm42688.h
View File

@ -4,10 +4,7 @@
#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
@ -69,58 +66,7 @@ enum icm42688_godr
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
@ -201,4 +147,257 @@ uint8_t MpuGetData(void);
#define ICM42688_INTF_CONFIG5 0x7B #define ICM42688_INTF_CONFIG5 0x7B
#define ICM42688_INTF_CONFIG6 0x7C #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 #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

82
foc_n.ioc
View File

@ -6,34 +6,43 @@ Mcu.CPN=STM32H750VBT6
Mcu.Family=STM32H7 Mcu.Family=STM32H7
Mcu.IP0=CORTEX_M7 Mcu.IP0=CORTEX_M7
Mcu.IP1=NVIC Mcu.IP1=NVIC
Mcu.IP10=USART1
Mcu.IP2=RCC Mcu.IP2=RCC
Mcu.IP3=SPI1 Mcu.IP3=SPI1
Mcu.IP4=SYS Mcu.IP4=SPI2
Mcu.IP5=TIM2 Mcu.IP5=SYS
Mcu.IP6=TIM3 Mcu.IP6=TIM2
Mcu.IP7=TIM5 Mcu.IP7=TIM3
Mcu.IP8=USART1 Mcu.IP8=TIM4
Mcu.IPNb=9 Mcu.IP9=TIM5
Mcu.IPNb=11
Mcu.Name=STM32H750VBTx Mcu.Name=STM32H750VBTx
Mcu.Package=LQFP100 Mcu.Package=LQFP100
Mcu.Pin0=PH0-OSC_IN (PH0) Mcu.Pin0=PH0-OSC_IN (PH0)
Mcu.Pin1=PH1-OSC_OUT (PH1) Mcu.Pin1=PH1-OSC_OUT (PH1)
Mcu.Pin10=PC8 Mcu.Pin10=PB15
Mcu.Pin11=PA9 Mcu.Pin11=PD12
Mcu.Pin12=PA10 Mcu.Pin12=PD13
Mcu.Pin13=VP_SYS_VS_Systick Mcu.Pin13=PD14
Mcu.Pin14=VP_TIM2_VS_ClockSourceINT Mcu.Pin14=PC6
Mcu.Pin15=VP_TIM3_VS_ClockSourceINT Mcu.Pin15=PC7
Mcu.Pin16=VP_TIM5_VS_ClockSourceINT Mcu.Pin16=PC8
Mcu.Pin17=PA9
Mcu.Pin18=PA10
Mcu.Pin19=VP_SYS_VS_Systick
Mcu.Pin2=PA0 Mcu.Pin2=PA0
Mcu.Pin20=VP_TIM2_VS_ClockSourceINT
Mcu.Pin21=VP_TIM3_VS_ClockSourceINT
Mcu.Pin22=VP_TIM4_VS_ClockSourceINT
Mcu.Pin23=VP_TIM5_VS_ClockSourceINT
Mcu.Pin3=PA1 Mcu.Pin3=PA1
Mcu.Pin4=PA2 Mcu.Pin4=PA2
Mcu.Pin5=PA5 Mcu.Pin5=PA5
Mcu.Pin6=PA6 Mcu.Pin6=PA6
Mcu.Pin7=PA7 Mcu.Pin7=PA7
Mcu.Pin8=PC6 Mcu.Pin8=PB13
Mcu.Pin9=PC7 Mcu.Pin9=PB14
Mcu.PinsNb=17 Mcu.PinsNb=24
Mcu.ThirdPartyNb=0 Mcu.ThirdPartyNb=0
Mcu.UserConstants= Mcu.UserConstants=
Mcu.UserName=STM32H750VBTx Mcu.UserName=STM32H750VBTx
@ -49,6 +58,7 @@ NVIC.PendSV_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
NVIC.PriorityGroup=NVIC_PRIORITYGROUP_4 NVIC.PriorityGroup=NVIC_PRIORITYGROUP_4
NVIC.SVCall_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false NVIC.SVCall_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
NVIC.SysTick_IRQn=true\:15\:0\:false\:false\:true\:false\:true\:false NVIC.SysTick_IRQn=true\:15\:0\:false\:false\:true\:false\:true\:false
NVIC.USART1_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true
NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
PA0.Locked=true PA0.Locked=true
PA0.Signal=S_TIM2_CH1_ETR PA0.Signal=S_TIM2_CH1_ETR
@ -71,12 +81,27 @@ PA7.Signal=SPI1_MOSI
PA9.Locked=true PA9.Locked=true
PA9.Mode=Asynchronous PA9.Mode=Asynchronous
PA9.Signal=USART1_TX PA9.Signal=USART1_TX
PB13.Locked=true
PB13.Mode=Full_Duplex_Master
PB13.Signal=SPI2_SCK
PB14.Locked=true
PB14.Mode=Full_Duplex_Master
PB14.Signal=SPI2_MISO
PB15.Locked=true
PB15.Mode=Full_Duplex_Master
PB15.Signal=SPI2_MOSI
PC6.Locked=true PC6.Locked=true
PC6.Signal=S_TIM3_CH1 PC6.Signal=S_TIM3_CH1
PC7.Locked=true PC7.Locked=true
PC7.Signal=S_TIM3_CH2 PC7.Signal=S_TIM3_CH2
PC8.Locked=true PC8.Locked=true
PC8.Signal=S_TIM3_CH3 PC8.Signal=S_TIM3_CH3
PD12.Locked=true
PD12.Signal=S_TIM4_CH1
PD13.Locked=true
PD13.Signal=S_TIM4_CH2
PD14.Locked=true
PD14.Signal=S_TIM4_CH3
PH0-OSC_IN\ (PH0).Mode=HSE-External-Oscillator PH0-OSC_IN\ (PH0).Mode=HSE-External-Oscillator
PH0-OSC_IN\ (PH0).Signal=RCC_OSC_IN PH0-OSC_IN\ (PH0).Signal=RCC_OSC_IN
PH1-OSC_OUT\ (PH1).Mode=HSE-External-Oscillator PH1-OSC_OUT\ (PH1).Mode=HSE-External-Oscillator
@ -109,7 +134,7 @@ ProjectManager.StackSize=0x400
ProjectManager.TargetToolchain=STM32CubeIDE ProjectManager.TargetToolchain=STM32CubeIDE
ProjectManager.ToolChainLocation= ProjectManager.ToolChainLocation=
ProjectManager.UnderRoot=true ProjectManager.UnderRoot=true
ProjectManager.functionlistsort=1-SystemClock_Config-RCC-false-HAL-false,2-MX_GPIO_Init-GPIO-false-HAL-true,3-MX_TIM2_Init-TIM2-false-HAL-true,4-MX_TIM3_Init-TIM3-false-HAL-true,5-MX_TIM5_Init-TIM5-false-HAL-true,6-MX_USART1_UART_Init-USART1-false-HAL-true,7-MX_SPI1_Init-SPI1-false-HAL-true,0-MX_CORTEX_M7_Init-CORTEX_M7-false-HAL-true ProjectManager.functionlistsort=1-SystemClock_Config-RCC-false-HAL-false,2-MX_GPIO_Init-GPIO-false-HAL-true,3-MX_TIM2_Init-TIM2-false-HAL-true,4-MX_TIM3_Init-TIM3-false-HAL-true,5-MX_TIM5_Init-TIM5-false-HAL-true,6-MX_USART1_UART_Init-USART1-false-HAL-true,7-MX_SPI1_Init-SPI1-false-HAL-true,8-MX_SPI2_Init-SPI2-false-HAL-true,0-MX_CORTEX_M7_Init-CORTEX_M7-false-HAL-true
RCC.ADCFreq_Value=50390625 RCC.ADCFreq_Value=50390625
RCC.AHB12Freq_Value=240000000 RCC.AHB12Freq_Value=240000000
RCC.AHB4Freq_Value=240000000 RCC.AHB4Freq_Value=240000000
@ -199,6 +224,12 @@ SH.S_TIM3_CH2.0=TIM3_CH2,PWM Generation2 CH2
SH.S_TIM3_CH2.ConfNb=1 SH.S_TIM3_CH2.ConfNb=1
SH.S_TIM3_CH3.0=TIM3_CH3,PWM Generation3 CH3 SH.S_TIM3_CH3.0=TIM3_CH3,PWM Generation3 CH3
SH.S_TIM3_CH3.ConfNb=1 SH.S_TIM3_CH3.ConfNb=1
SH.S_TIM4_CH1.0=TIM4_CH1,PWM Generation1 CH1
SH.S_TIM4_CH1.ConfNb=1
SH.S_TIM4_CH2.0=TIM4_CH2,PWM Generation2 CH2
SH.S_TIM4_CH2.ConfNb=1
SH.S_TIM4_CH3.0=TIM4_CH3,PWM Generation3 CH3
SH.S_TIM4_CH3.ConfNb=1
SPI1.BaudRatePrescaler=SPI_BAUDRATEPRESCALER_32 SPI1.BaudRatePrescaler=SPI_BAUDRATEPRESCALER_32
SPI1.CLKPhase=SPI_PHASE_2EDGE SPI1.CLKPhase=SPI_PHASE_2EDGE
SPI1.CalculateBaudRate=5.0 MBits/s SPI1.CalculateBaudRate=5.0 MBits/s
@ -207,6 +238,16 @@ SPI1.Direction=SPI_DIRECTION_2LINES
SPI1.IPParameters=VirtualType,Mode,Direction,BaudRatePrescaler,CalculateBaudRate,DataSize,CLKPhase SPI1.IPParameters=VirtualType,Mode,Direction,BaudRatePrescaler,CalculateBaudRate,DataSize,CLKPhase
SPI1.Mode=SPI_MODE_MASTER SPI1.Mode=SPI_MODE_MASTER
SPI1.VirtualType=VM_MASTER SPI1.VirtualType=VM_MASTER
SPI2.BaudRatePrescaler=SPI_BAUDRATEPRESCALER_8
SPI2.CLKPhase=SPI_PHASE_2EDGE
SPI2.CLKPolarity=SPI_POLARITY_HIGH
SPI2.CalculateBaudRate=20.0 MBits/s
SPI2.DataSize=SPI_DATASIZE_8BIT
SPI2.Direction=SPI_DIRECTION_2LINES
SPI2.IPParameters=VirtualType,Mode,Direction,CalculateBaudRate,DataSize,CLKPolarity,CLKPhase,BaudRatePrescaler,NSSPolarity
SPI2.Mode=SPI_MODE_MASTER
SPI2.NSSPolarity=SPI_NSS_POLARITY_HIGH
SPI2.VirtualType=VM_MASTER
TIM2.Channel-PWM\ Generation1\ CH1=TIM_CHANNEL_1 TIM2.Channel-PWM\ Generation1\ CH1=TIM_CHANNEL_1
TIM2.Channel-PWM\ Generation2\ CH2=TIM_CHANNEL_2 TIM2.Channel-PWM\ Generation2\ CH2=TIM_CHANNEL_2
TIM2.Channel-PWM\ Generation3\ CH3=TIM_CHANNEL_3 TIM2.Channel-PWM\ Generation3\ CH3=TIM_CHANNEL_3
@ -219,6 +260,11 @@ TIM3.Channel-PWM\ Generation3\ CH3=TIM_CHANNEL_3
TIM3.CounterMode=TIM_COUNTERMODE_CENTERALIGNED1 TIM3.CounterMode=TIM_COUNTERMODE_CENTERALIGNED1
TIM3.IPParameters=Channel-PWM Generation1 CH1,Channel-PWM Generation2 CH2,Channel-PWM Generation3 CH3,Period,CounterMode TIM3.IPParameters=Channel-PWM Generation1 CH1,Channel-PWM Generation2 CH2,Channel-PWM Generation3 CH3,Period,CounterMode
TIM3.Period=4800 TIM3.Period=4800
TIM4.Channel-PWM\ Generation1\ CH1=TIM_CHANNEL_1
TIM4.Channel-PWM\ Generation2\ CH2=TIM_CHANNEL_2
TIM4.Channel-PWM\ Generation3\ CH3=TIM_CHANNEL_3
TIM4.IPParameters=Channel-PWM Generation1 CH1,Channel-PWM Generation2 CH2,Channel-PWM Generation3 CH3,Period
TIM4.Period=255
TIM5.IPParameters=Prescaler TIM5.IPParameters=Prescaler
TIM5.Prescaler=24 TIM5.Prescaler=24
USART1.IPParameters=VirtualMode-Asynchronous USART1.IPParameters=VirtualMode-Asynchronous
@ -229,6 +275,8 @@ VP_TIM2_VS_ClockSourceINT.Mode=Internal
VP_TIM2_VS_ClockSourceINT.Signal=TIM2_VS_ClockSourceINT VP_TIM2_VS_ClockSourceINT.Signal=TIM2_VS_ClockSourceINT
VP_TIM3_VS_ClockSourceINT.Mode=Internal VP_TIM3_VS_ClockSourceINT.Mode=Internal
VP_TIM3_VS_ClockSourceINT.Signal=TIM3_VS_ClockSourceINT VP_TIM3_VS_ClockSourceINT.Signal=TIM3_VS_ClockSourceINT
VP_TIM4_VS_ClockSourceINT.Mode=Internal
VP_TIM4_VS_ClockSourceINT.Signal=TIM4_VS_ClockSourceINT
VP_TIM5_VS_ClockSourceINT.Mode=Internal VP_TIM5_VS_ClockSourceINT.Mode=Internal
VP_TIM5_VS_ClockSourceINT.Signal=TIM5_VS_ClockSourceINT VP_TIM5_VS_ClockSourceINT.Signal=TIM5_VS_ClockSourceINT
board=custom board=custom