黑洞飞控代码分析（一）

最新推荐文章于 2024-01-06 17:50:33 发布

JRlipton

最新推荐文章于 2024-01-06 17:50:33 发布

阅读量1k

点赞数

本文链接：https://blog.csdn.net/wang2012113132/article/details/78956279

版权

下面分析一下黑洞飞控的源代码

代码版本1.0（具体我也不知道）

IDE keil5

飞控主MCU是stm32，先main

int main(void)
{			
	System_Init();										//ÏµÍ³³õÊ¼»¯		
	while(1)													//¿ÕÑ»·
	{					
  }	 			  
}

套路就是先系统初始化，在while（1）死循环

系统初始化

/*
 * 函数名：System_Init
 * 描述  ：系统初始化函数
 * 输入  ：无
 * 输出  ：无
 */  
void System_Init(void)
{
	LED_Init();													//LED状态灯初始化
	PID_Init();													//PID参数初始化
	PWM_Out_Init(3999,71,0,0,0,0);	//PWM输出初始化
	USART3_Init(460800);								//串口初始化
	while(!MPU6500_Init());							//等待MPU6500初始化成功
	LED_StartShow();										//两个作用：一是延时，二是状态显示
	Systick_Init();											//开启1ms中断
	PWM_In_Init(0xffff,72	-	1);       	//开启输入捕获，定时精度为1us
}

首先是LDE的初始化

void LED_Init(void)
{
	GPIO_InitTypeDef GPIO_InitStruct;	
	
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB|RCC_APB2Periph_GPIOC, ENABLE);
	
	GPIO_InitStruct.GPIO_Pin = GPIO_Pin_12;
        GPIO_InitStruct.GPIO_Mode = GPIO_Mode_Out_PP;
        GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
        GPIO_Init(GPIOB, &GPIO_InitStruct);
	GPIO_InitStruct.GPIO_Pin = GPIO_Pin_13;
	GPIO_Init(GPIOC, &GPIO_InitStruct);
	
}

初始化管脚，两个
接下来PID 的初始化

void PID_Init(void)
{
//角度环	
	PID_Roll_Angle.P = 14;
	PID_Roll_Angle.I = 0;
	PID_Roll_Angle.D = 0.25;
	
	PID_Pitch_Angle.P = 14;
	PID_Pitch_Angle.I = 0;
	PID_Pitch_Angle.D = 0.25;
	
	PID_Yaw_Angle.P = 20;
	PID_Yaw_Angle.I = 0;
	PID_Yaw_Angle.D = 1;
		
//角速度换	
	PID_Roll_Rate.P = 0;
	PID_Roll_Rate.I = 0;
	PID_Roll_Rate.D = 0;
	
	PID_Pitch_Rate.P = 0;
	PID_Pitch_Rate.I = 0;
	PID_Pitch_Rate.D = 0;
	
	PID_Yaw_Rate.P = 0;
	PID_Yaw_Rate.I = 0;
	PID_Yaw_Rate.D = 0;


//初始化清零
	PID_Roll_Angle.Pout = 0;
	PID_Roll_Angle.Iout = 0;
	PID_Roll_Angle.Dout = 0;
	
	PID_Pitch_Angle.Pout = 0;
	PID_Pitch_Angle.Iout = 0;
	PID_Pitch_Angle.Dout = 0;
	
	PID_Yaw_Angle.Pout = 0;
	PID_Yaw_Angle.Iout = 0;
	PID_Yaw_Angle.Dout = 0;
	
	PID_Roll_Rate.Pout = 0;
	PID_Roll_Rate.Iout = 0;
	PID_Roll_Rate.Dout = 0;
	
	PID_Pitch_Rate.Pout = 0;
	PID_Pitch_Rate.Iout = 0;
	PID_Pitch_Rate.Dout = 0;
	
	PID_Yaw_Rate.Pout = 0;
	PID_Yaw_Rate.Iout = 0;
	PID_Yaw_Rate.Dout = 0;
}

在代码旁边作者赋了调PID的口诀贴一下

看代码的意思控制是串级pid，只有姿态控制

接下来是pwm的初始化

PWM_Out_Init(3999,71,0,0,0,0);

/*
 * 函数名：TIM4_Init
 * 描述  ：定时器4的PWM输出初始化函数
 * 输入  ：arr：自动重装载寄存器周期的值；psc：时钟频率除数的预分频值；CCRx：对应思路PWM输出占空比
 * 输出  ：无
 */ 
void PWM_Out_Init(u16 arr, u16 psc,u16	CCR1,u16	CCR2,u16	CCR3,u16	CCR4)//CCR为16位的数
{	 
		GPIO_InitTypeDef GPIO_InitStructure;
		TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
		TIM_OCInitTypeDef  TIM_OCInitStructure;
	  
		RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);
  	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE); 
	  
		GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7| GPIO_Pin_8| GPIO_Pin_9;
		GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
		GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
		GPIO_Init(GPIOB, &GPIO_InitStructure); 
			
  	TIM_TimeBaseStructure.TIM_Period = arr;
  	TIM_TimeBaseStructure.TIM_Prescaler = psc;
  	TIM_TimeBaseStructure.TIM_ClockDivision = 0;
  	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
  	TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);
	
  	//通道1
  	TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
  	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
  	TIM_OCInitStructure.TIM_Pulse = CCR1;
  	TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
  	TIM_OC1Init(TIM4, &TIM_OCInitStructure);
  	TIM_OC1PreloadConfig(TIM4, TIM_OCPreload_Enable);
		
		//通道2 	
  	TIM_OCInitStructure.TIM_Pulse = CCR2;
  	TIM_OC2Init(TIM4, &TIM_OCInitStructure);
  	TIM_OC2PreloadConfig(TIM4, TIM_OCPreload_Enable);
		
		//通道3
  	TIM_OCInitStructure.TIM_Pulse = CCR3;
  	TIM_OC3Init(TIM4, &TIM_OCInitStructure);
  	TIM_OC3PreloadConfig(TIM4, TIM_OCPreload_Enable);
		
		//通道4
  	TIM_OCInitStructure.TIM_Pulse = CCR4;
  	TIM_OC4Init(TIM4, &TIM_OCInitStructure);
  	TIM_OC4PreloadConfig(TIM4, TIM_OCPreload_Enable);

		TIM_ARRPreloadConfig(TIM4,ENABLE);
		TIM_Cmd(TIM4, ENABLE);		
}



用TIM4产生4路的pwm波，通过GPIOB6，,7，8，9输出

这里初始值的话

TIM_TimeBaseStructure.TIM_Period = 3999;

TIM_TimeBaseStructure.TIM_Prescaler = 71;



因为系统时钟是72MHZ，时钟不分割，预分频系数72，重装载值4000，这里说明这个pwm的频率为250HZ，那么pwm波的周期是4000us


对于串口的初始化
/*
 * 函数名：USART3_Init
 * 描述  ：串口3初始化函数
 * 输入  ：波特率
 * 输出  ：无
 */  	 
void USART3_Init(u32 bound)
{
	//GPIO端口设置
  GPIO_InitTypeDef GPIO_InitStructure;
	USART_InitTypeDef USART_InitStructure;
	 
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB|RCC_APB2Periph_AFIO,ENABLE);
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE);

	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10;       
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;																
	GPIO_Init(GPIOB, &GPIO_InitStructure); 																				
  
	//USART 初始化设置
	USART_InitStructure.USART_BaudRate = bound;																		 //波特率
	USART_InitStructure.USART_WordLength = USART_WordLength_8b;										 //字长为8位数据格式
	USART_InitStructure.USART_StopBits = USART_StopBits_1;											   //一个停止位
	USART_InitStructure.USART_Parity = USART_Parity_No;														 //无奇偶校验位
	USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;//无硬件数据流控制
	USART_InitStructure.USART_Mode =  USART_Mode_Tx | USART_Mode_Rx;							 //收发模式

  USART_Init(USART3, &USART_InitStructure); 																		 //初始化串口

  USART_Cmd(USART3, ENABLE);                																		 //使能串口 
}





这里只初始化了GPIOB-10，应该这里用它既做收发又做接受

mpu6500初始化，这里用的是spi总线



 * 函数名：MPU6500_Init
 * 描述  ：MPU6500初始化函数
 * 输入  ：无
 * 输出  ：0：初始化失败 1：初始化成功
 */ 
u8 MPU6500_Init(void)
{
	SPI1_Init();																//MPU6500 IO口和SPI初始化
	
	if(MPU6500_Read_Reg(WHO_AM_I) == 0x70)			//正确读取到6500的地址
	{		
		MPU6500_Write_Reg(PWR_MGMT_1,0X80);   		//电源管理,复位MPU6500
		Delay_Ms(100);
		MPU6500_Write_Reg(SIGNAL_PATH_RESET,0X07);//陀螺仪、加速度计、温度计复位
		Delay_Ms(100);
		MPU6500_Write_Reg(PWR_MGMT_1,0X01);   		//选择时钟源
		MPU6500_Write_Reg(PWR_MGMT_2,0X00);   		//使能加速度计和陀螺仪
		MPU6500_Write_Reg(CONFIG,0X02);						//低通滤波器 0x02 92hz (3.9ms delay) fs=1khz
		MPU6500_Write_Reg(SMPLRT_DIV,0X00);				//采样率1000/(1+0)=1000HZ
		MPU6500_Write_Reg(GYRO_CONFIG,0X18);  		//陀螺仪测量范围 0X18 正负2000度
		MPU6500_Write_Reg(ACCEL_CONFIG,0x10); 		//加速度计测量范围 0X00 正负8g
		MPU6500_Write_Reg(ACCEL_CONFIG2,0x00);		//加速度计速率1khz 滤波器460hz (1.94ms delay)
		return 1;
	}
	else return 0;
}





下面就是点亮LED灯了


LED_StartShow
void LED_StartShow(void)
{
	int i = 0;
	
	for(; i<5 ; i++)
	{
		LED_LEFT_BACK(1);
		LED_RIGHT_BACK(0);
		Delay_Ms(200);
		LED_LEFT_BACK(0);
		LED_RIGHT_BACK(1);
		Delay_Ms(200);
	}
	
	LED_LEFT_BACK(1);
	LED_RIGHT_BACK(1);
}5次循环

LED_LEFT_BACK(1);LED_RIGHT_BACK(0);是两个宏，1的时候点亮，0的时候熄灭，延时200ms，交替

#define LED_LEFT_BACK(X)  (X==0)?GPIO_ResetBits(GPIOC,GPIO_Pin_13):GPIO_SetBits(GPIOC,GPIO_Pin_13)
#define LED_RIGHT_BACK(X)  (X==0)?GPIO_ResetBits(GPIOB,GPIO_Pin_12):GPIO_SetBits(GPIOB,GPIO_Pin_12)这里有一个延时函数可以看看


void Delay_Ms(u16 ms)
{    
   u16 i=0;  
   while(ms--)
   {
      i=12000;
      while(i--) ;    
   }
 }这里是自写的延时函数，并不是有定时器延时

接下来是系统时钟的初始化

/*
 * 函数名：SysTick_Init
 * 描述  ：启动系统滴答定时器 SysTick
 * 输入  ：无
 * 输出  ：无
 */
void Systick_Init(void)
{
	//SystemFrequency / 1000    1ms中断一次
	//SystemFrequency / 500	    2ms中断一次
	 
	if (SysTick_Config(SystemCoreClock / 1000))
	{ 
		while (1);
	}
}


延时1ms

因为有定义：#define SYSCLK_FREQ_72MHz  72000000

所以定义有：

#ifdef SYSCLK_FREQ_HSE
  uint32_t SystemCoreClock         = SYSCLK_FREQ_HSE;        /*!< System Clock Frequency (Core Clock) */
#elif defined SYSCLK_FREQ_24MHz
  uint32_t SystemCoreClock         = SYSCLK_FREQ_24MHz;        /*!< System Clock Frequency (Core Clock) */
#elif defined SYSCLK_FREQ_36MHz
  uint32_t SystemCoreClock         = SYSCLK_FREQ_36MHz;        /*!< System Clock Frequency (Core Clock) */
#elif defined SYSCLK_FREQ_48MHz
  uint32_t SystemCoreClock         = SYSCLK_FREQ_48MHz;        /*!< System Clock Frequency (Core Clock) */
#elif defined SYSCLK_FREQ_56MHz
  uint32_t SystemCoreClock         = SYSCLK_FREQ_56MHz;        /*!< System Clock Frequency (Core Clock) */
#elif defined SYSCLK_FREQ_72MHz
  uint32_t SystemCoreClock         = SYSCLK_FREQ_72MHz;        /*!< System Clock Frequency (Core Clock) */
#else /*!< HSI Selected as System Clock source */
  uint32_t SystemCoreClock         = HSI_VALUE;        /*!< System Clock Frequency (Core Clock) */
#endif
所以 uint32_t SystemCoreClock         = SYSCLK_FREQ_72MHz;

所以系统时钟输入为SysTick_Config(SystemCoreClock / 1000)=SysTick_Config(72000)

接下来输入捕获初始化

PWM_In_Init(0xffff,72-1);
具体过程
void PWM_In_Init(u16 arr,u16 psc)
{	 
   TIM2_PWM_In_Init(arr,psc);
	 TIM3_PWM_In_Init(arr,psc);
}
以TIM2为例
/*
 * 函数名：TIM2_PWM_In_Init
 * 描述  ：定时器2输入捕获初始化函数
 * 输入  ：arr：自动重装载寄存器周期的值；psc：时钟频率除数的预分频值
 * 输出  ：无
 */ 
void TIM2_PWM_In_Init(u16 arr,u16 psc)
{
	GPIO_InitTypeDef GPIO_InitStructure;
	TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
	TIM_ICInitTypeDef  TIM2_ICInitStructure;
  NVIC_InitTypeDef NVIC_InitStructure;

	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);													//使能TIM2时钟
 	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);   											//使能GPIOA时钟
	
	GPIO_InitStructure.GPIO_Pin  = GPIO_Pin_0|GPIO_Pin_1|GPIO_Pin_2|GPIO_Pin_3;  	//PA0 清除之前设置  
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPD;           											//上升沿捕获，故设置为下拉输入
	GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz; 
	GPIO_Init(GPIOA, &GPIO_InitStructure);
	
	GPIO_ResetBits(GPIOA,GPIO_Pin_0);
	GPIO_ResetBits(GPIOA,GPIO_Pin_1);
	GPIO_ResetBits(GPIOA,GPIO_Pin_2);
	GPIO_ResetBits(GPIOA,GPIO_Pin_3);
	 
	TIM_TimeBaseStructure.TIM_Period = arr;     																	//设定计数器自动重装值 
	TIM_TimeBaseStructure.TIM_Prescaler =psc; 																		//预分频器   
	TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; 											//设置时钟分割:TDTS = Tck_tim
	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;  									//TIM向上计数模式
	TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure); 															//初始化TIMx的时间基数单位
  
	//初始化TIM2通道1输入捕获参数
	TIM2_ICInitStructure.TIM_Channel = TIM_Channel_1 ;  													//选择输入端 IC1映射到TI1上
  TIM2_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;									//上升沿捕获
  TIM2_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI; 							//映射到TI1上
 	TIM2_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;											  //配置输入分频,不分频 
 	TIM2_ICInitStructure.TIM_ICFilter = 0x00;																			//配置输入滤波器，不滤波
  TIM_ICInit(TIM2, &TIM2_ICInitStructure);
	
	//初始化TIM2通道2输入捕获参数
	TIM2_ICInitStructure.TIM_Channel = TIM_Channel_2 ; 														//选择输入端 IC1映射到TI1上
  TIM2_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;									//上升沿捕获
  TIM2_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI; 							//映射到TI1上
  TIM2_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;	 											//配置输入分频,不分频 
  TIM2_ICInitStructure.TIM_ICFilter = 0x00;																			//配置输入滤波器,不滤波
  TIM_ICInit(TIM2, &TIM2_ICInitStructure);
	
	//初始化TIM2通道3输入捕获参数
	TIM2_ICInitStructure.TIM_Channel = TIM_Channel_3 ; 														//选择输入端 IC1映射到TI1上
  TIM2_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;									//上升沿捕获
  TIM2_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI; 							//映射到TI1上
  TIM2_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;	 											//配置输入分频,不分频 
 	TIM2_ICInitStructure.TIM_ICFilter = 0x00;																			//配置输入滤波器，不滤波
 	TIM_ICInit(TIM2, &TIM2_ICInitStructure);
	
	//初始化TIM2通道4输入捕获参数
	TIM2_ICInitStructure.TIM_Channel = TIM_Channel_4;															//选择输入端 IC1映射到TI1上
	TIM2_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;									//上升沿捕获
	TIM2_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI; 							//映射到TI1上
	TIM2_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;	 											//配置输入分频,不分频 
	TIM2_ICInitStructure.TIM_ICFilter = 0x00;																			//配置输入滤波器，不滤波
	TIM_ICInit(TIM2, &TIM2_ICInitStructure);
		
	//中断分组初始化
	NVIC_InitStructure.NVIC_IRQChannel = TIM2_IRQn  ;  														//TIM2中断
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 3;  										//先占优先级
	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2;  													//从优先级
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; 															//IRQ通道被使能
	NVIC_Init(&NVIC_InitStructure);  																							//初始化外设NVIC寄存器 

	TIM_ITConfig(TIM2,TIM_IT_Update|TIM_IT_CC1|TIM_IT_CC2|TIM_IT_CC3|TIM_IT_CC4 ,ENABLE);//允许更新中断 ,允许CC1IE捕获中断	
	
 	TIM_Cmd(TIM2,ENABLE ); 																												//使能定时器2
}

JRlipton

关注

0
点赞
踩
4

收藏

觉得还不错? 一键收藏
0
评论
黑洞飞控代码分析（一）

下面分析一下黑洞飞控的源代码代码版本1.0（具体我也不知道）IDE keil5飞控主MCU是stm32，先mainint main(void){ System_Init(); //ÏµÍ³³õÊ¼»¯ while(1) //¿ÕÑ»· { } }套路就是先系统初始化，在while（1）死循
复制链接

扫一扫