【STM32】飞控设计

【一些入门知识】

1.飞行原理

【垂直运动】

mg>F1+F2+F3+F4,此时做下降加速飞行
mg<F1+F2+F3+F4,此时做升高加速飞行
mg=F1+F2+F3+F4 ,此时垂直上保持匀速飞行。

【偏航飞行】

ω 4 + ω 2 ≠ ω 1+ ω 3 就会产生水平旋转

【俯仰飞行】

F1+F4<F2+F3 向前飞行
F1+F4>F2+F3向后飞行

【横滚飞行】

F4+F3>F1+F2向右飞行
F4+F3<F1+F2 向左飞行

2.串级PID

3.飞控的控制系统

4.姿态解算

一.硬件设计(简)

【主控】

1.电源:3.7V锂电池供电 - DCDC升压至5V - LDO稳压3.3V

2.USB - 上位机

3.SPI - NRF24L01无线通讯

4.I2C - MPU6050陀螺仪

5.4个PWM

6.主控STM32F103C8T6

【遥控】

1.电源:3.7V锂电池供电 - LDO稳压3.3V

2.I2C - AT24CO2

3.4个ADC - 两个遥感

4.8个IO口 - 8个按键

5.SPI - NRF24L01无线通讯

6.主控STM32F103C8T6

二.主控程序

【MPU6050读取飞控三轴加速度、角速度 并且 卡尔曼滤波】

通过 MPU6050 寄存器手册:我们需要读取的三轴加速度和三轴角速度位于寄存器 0x3B~0X48,读取数据后,需要合成 16bit 的数据。

cs 复制代码
//从 0x3B 读取 6 个字节放到 buffer 里面
#define Acc_Read() i2cRead(0x68, 0X3B,6,buffer)

//从 0x43 读取 6 个字节放到 buffer 里面
#define Gyro_Read() i2cRead(0x68, 0x43,6,&buffer[6])

void MpuGetData(void) //读取陀螺仪数据加滤波
{
   uint8_t i;
   uint8_t buffer[12];

   Acc_Read();//读取加速度
   Gyro_Read();//读取角速度

  for(i=0;i<6;i++)
  {
    //整合为 16bit,并减去水平静止校准值
    pMpu[i] = (((int16_t)buffer[i<<1] << 8) | buffer[(i<<1)+1])-MpuOffset[i];

    if(i < 3)//对加速度做卡尔曼滤波
    {
      {
         //卡尔曼滤波的数据初始化,这个 8192 是初始化默认 1 个 g 的加速度
         static struct _1_ekf_filter 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}};
         kalman_1(&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 last_mpuData[3];
     //滤波并保存滤波数据 
     last_mpuData[k] = last_mpuData[k] * (1 - factor) + pMpu[i] * factor; 
     pMpu[i] = last_mpuData[k];//滤波输出
     }
   }
}

【遥控数据解析】

cs 复制代码
void RC_Analy(void)  
{
		static uint16_t cnt;

	if(NRF24L01_RxPacket(RC_rxData)==SUCCESS)
	{ 	
		uint8_t i;
		uint8_t CheckSum=0;
		uint16_t thr;
		cnt = 0;
		for(i=0;i<31;i++)
		{
			CheckSum +=  RC_rxData[i]; //检查数据的数量是否是31个
		}
		if(RC_rxData[31]==CheckSum && RC_rxData[0]==0xAA && RC_rxData[1]==0xAF)  //如果接收到的遥控数据正确
		{
			    Remote.roll = ((uint16_t)RC_rxData[4]<<8) | RC_rxData[5];  //通道1
				Remote.roll = LIMIT(Remote.roll,1000,2000);
				Remote.pitch = ((uint16_t)RC_rxData[6]<<8) | RC_rxData[7];  //通道2
				Remote.pitch = LIMIT(Remote.pitch,1000,2000);
				Remote.thr = 	((uint16_t)RC_rxData[8]<<8) | RC_rxData[9];   //通道3
				Remote.thr = 	LIMIT(Remote.thr,1000,2000);
				Remote.yaw =  ((uint16_t)RC_rxData[10]<<8) | RC_rxData[11];   //通道4
				Remote.yaw =  LIMIT(Remote.yaw,1000,2000);
				Remote.AUX1 =  ((uint16_t)RC_rxData[12]<<8) | RC_rxData[13];   //通道5  左上角按键都属于通道5  
				Remote.AUX1 =  LIMIT(Remote.AUX1,1000,2000);
				Remote.AUX2 =  ((uint16_t)RC_rxData[14]<<8) | RC_rxData[15];   //通道6  右上角按键都属于通道6 
				Remote.AUX2 =  LIMIT(Remote.AUX2,1000,2000);
				Remote.AUX3 =  ((uint16_t)RC_rxData[16]<<8) | RC_rxData[17];   //通道7  左下边按键都属于通道7 
				Remote.AUX3 =  LIMIT(Remote.AUX3,1000,2000);
				Remote.AUX4 =  ((uint16_t)RC_rxData[18]<<8) | RC_rxData[19];   //通道8  右下边按键都属于通道6  
				Remote.AUX4 = LIMIT(Remote.AUX4,1000,4000);	
				
				{
							const float roll_pitch_ratio = 0.04f;
							const float yaw_ratio =   0.3f;	
					
							pidPitch.desired =-(Remote.pitch-1500)*roll_pitch_ratio;	 //将遥杆值作为飞行角度的期望值
							pidRoll.desired = -(Remote.roll-1500)*roll_pitch_ratio;
					    if(Remote.yaw>1820)
							{
								pidYaw.desired -= yaw_ratio;	
							}
							else if(Remote.yaw <1180)
							{
								pidYaw.desired += yaw_ratio;	
							}						
				}
				remote_unlock();
		}
  }
//如果3秒没收到遥控数据,则判断遥控信号丢失,飞控在任何时候停止飞行,避免伤人。
//意外情况,使用者可紧急关闭遥控电源,飞行器会在3秒后立即关闭,避免伤人。
//立即关闭遥控,如果在飞行中会直接掉落,可能会损坏飞行器。
	else
	{
	
		
		cnt++;
		if(cnt>500)
		{
			cnt = 0;
			ALL_flag.unlock = 0; 
			NRF24L01_init();
		}
	}
}

【PID控制器的设计】

cpp 复制代码
void FlightPidControl(float dt)
{
	volatile static uint8_t status=WAITING_1;

	switch(status)
	{		
		case WAITING_1: //等待解锁
			if(ALL_flag.unlock)
			{
				status = READY_11;	
			}			
			break;
		case READY_11:  //准备进入控制
			pidRest(pPidObject,6); //批量复位PID数据,防止上次遗留的数据影响本次控制

			Angle.yaw = pidYaw.desired =  pidYaw.measured = 0;   //锁定偏航角
		
			status = PROCESS_31;
		
			break;			
		case PROCESS_31: //正式进入控制
			if(Angle.pitch<-50||Angle.pitch>50||Angle.roll<-50||Angle.roll>50)//倾斜检测,大角度判定为意外情况,则紧急上锁	
					if(Remote.thr>1200)//当油门的很低时不做倾斜检测
						ALL_flag.unlock = EMERGENT;//打入紧急情况
			
      pidRateX.measured = MPU6050.gyroX * Gyro_G; //内环测量值 角度/秒
			pidRateY.measured = MPU6050.gyroY * Gyro_G;
			pidRateZ.measured = MPU6050.gyroZ * Gyro_G;
		
			pidPitch.measured = Angle.pitch; //外环测量值 单位:角度
		  pidRoll.measured = Angle.roll;
			pidYaw.measured = Angle.yaw;
		
		 	pidUpdate(&pidRoll,dt);    //调用PID处理函数来处理外环	横滚角PID		
			pidRateX.desired = pidRoll.out; //将外环的PID输出作为内环PID的期望值即为串级PID
			pidUpdate(&pidRateX,dt);  //再调用内环

		 	pidUpdate(&pidPitch,dt);    //调用PID处理函数来处理外环	俯仰角PID	
			pidRateY.desired = pidPitch.out;  
			pidUpdate(&pidRateY,dt); //再调用内环

			CascadePID(&pidRateZ,&pidYaw,dt);	//也可以直接调用串级PID函数来处理
			break;
		case EXIT_255:  //退出控制
			pidRest(pPidObject,6);
			status = WAITING_1;//返回等待解锁
		  break;
		default:
			status = EXIT_255;
			break;
	}
	if(ALL_flag.unlock == EMERGENT) //意外情况,请使用遥控紧急上锁,飞控就可以在任何情况下紧急中止飞行,锁定飞行器,退出PID控制
		status = EXIT_255;
}

【4路PWM电机驱动】

cs 复制代码
void TIM2_PWM_Config(void)
{
 TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
 TIM_OCInitTypeDef TIM_OCInitStructure;
 GPIO_InitTypeDef GPIO_InitStructure;
 /* 使能 GPIOA 时钟时钟 */
 RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
 GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_2 |
 GPIO_Pin_3;
 GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
 GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
 GPIO_Init(GPIOA, &GPIO_InitStructure);
 /* 使能定时器 2 时钟 */
 RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
 /* Time base configuration */
 TIM_TimeBaseStructure.TIM_Period = 999; //定时器计数周期 0-999 1000
 TIM_TimeBaseStructure.TIM_Prescaler = 8; //设置预分频:8+1 分频 8K PWM 频率
 TIM_TimeBaseStructure.TIM_ClockDivision = 0; //设置时钟分频系数:不分频
 TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; //向上计数模式
 TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);
 /* PWM1 Mode configuration: Channel */
 TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; //配置为 PWM 模式 1
 TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
 TIM_OCInitStructure.TIM_Pulse = 0;
 //设置跳变值,当计数器计数到这个值时,电平发生跳变(即占空比) 初始值 0
 TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
 //当定时器计数值小于定时设定值时为高电平
 /* 使能通道 1 */
 TIM_OC1Init(TIM2, &TIM_OCInitStructure);
 TIM_OC1PreloadConfig(TIM2, TIM_OCPreload_Enable);
 /* 使能通道 2 */
 TIM_OC2Init(TIM2, &TIM_OCInitStructure);
 TIM_OC2PreloadConfig(TIM2, TIM_OCPreload_Enable);
 /* 使能通道 3 */
 TIM_OC3Init(TIM2, &TIM_OCInitStructure);
 TIM_OC3PreloadConfig(TIM2, TIM_OCPreload_Enable);
/* 使能通道 4 */
 TIM_OC4Init(TIM2, &TIM_OCInitStructure);
 TIM_OC4PreloadConfig(TIM2, TIM_OCPreload_Enable);
 TIM_ARRPreloadConfig(TIM2, ENABLE); // 使能 TIM2 重载寄存器 ARR
 TIM_Cmd(TIM2, ENABLE); //使能定时器 2
}

【解锁 - 启动步骤 - 电机动力分配】

cs 复制代码
void MotorControl(void)
{	
	volatile static uint8_t status=WAITING_1;
	
	
	if(ALL_flag.unlock == EMERGENT) //意外情况,请使用遥控紧急上锁,飞控就可以在任何情况下紧急中止飞行,锁定飞行器,退出PID控制
		status = EXIT_255;	
	switch(status)
	{		
		case WAITING_1: //等待解锁	
			MOTOR1 = MOTOR2 = MOTOR3 = MOTOR4 = 0;  //如果锁定,则电机输出都为0
			if(ALL_flag.unlock)
			{
				status = WAITING_2;
			}
		case WAITING_2: //解锁完成后判断使用者是否开始拨动遥杆进行飞行控制
			if(Remote.thr>1100)
			{
				low_thr_cnt_quiet=0;
				low_thr_cnt=0;
				pidRest(pPidObject,6);
				status = PROCESS_31;
			}
			break;
		case PROCESS_31:
			{
				int16_t temp,thr;
				temp = Remote.thr -1000; //油门+定高输出值
				//油门比例规划
				thr = 250+0.45f * temp;
			
				if(temp<10) //自动关停判断
				{
					if(low_thr_cnt<1500)
					low_thr_cnt++;
					thr = thr-(low_thr_cnt*0.6);//油门摇杆值慢慢降为0 
					if(MPU6050.accZ<8500&&MPU6050.accZ>7800)
					{
						low_thr_cnt++;
						if(low_thr_cnt>600)//1800ms
						{
							thr = 0;
							
							pidRest(pPidObject,6);
							MOTOR1 = MOTOR2 = MOTOR3 = MOTOR4 =0;
							status = WAITING_2;
							
							break;
						}
					}
				} 
				else low_thr_cnt=0;
				MOTOR1 = MOTOR2 = MOTOR3 = MOTOR4 = LIMIT(thr,0,700); //留100给姿态控制
					
//以下输出的脉冲分配取决于电机PWM分布与飞控坐标体系。请看飞控坐标体系图解,与四个电机PWM分布分布	
//           机头      
//   PWM3     ♂       PWM1
//      *             *
//      	*       *
//    		  *   *
//      	    *  
//    		  *   *
//         *        *
//      *             *
//    PWM4           PWM2			
//		pidRateX.out 横滚角串级PID输出 控制左右,可以看出1 2和3 4,左右两组电机同增同减
//      pidRateY.out 俯仰角串级PID输出 控制前后,可以看出2 3和1 4,前后两组电机同增同减
//		pidRateZ.out 横滚角串级PID输出 控制旋转,可以看出2 4和1 3,两组对角线电机同增同减	

// 正负号取决于算法输出 比如输出是正的话  往前飞必然是尾巴两个电机增加,往右飞必然是左边两个电机增加		

				MOTOR1 +=    + pidRateX.out + pidRateY.out + pidRateZ.out;//; 姿态输出分配给各个电机的控制量
				MOTOR2 +=    + pidRateX.out - pidRateY.out - pidRateZ.out ;//;
				MOTOR3 +=    - pidRateX.out + pidRateY.out - pidRateZ.out;
				MOTOR4 +=    - pidRateX.out - pidRateY.out + pidRateZ.out;//;
			}	
			break;
		case EXIT_255:
			MOTOR1 = MOTOR2 = MOTOR3 = MOTOR4 = 0;  //如果锁定,则电机输出都为0
			status = WAITING_1;	
			break;
		default:
			break;
	}
	
	
	TIM2->CCR1 = LIMIT(MOTOR1,0,1000);  //更新PWM
	TIM2->CCR2 = LIMIT(MOTOR2,0,1000);
	TIM2->CCR3 = LIMIT(MOTOR3,0,1000);
	TIM2->CCR4 = LIMIT(MOTOR4,0,1000);
} 

【水平校准】

MPU6050 获取的数值要减去水平静止校准值才是真正的飞控可用数据

cs 复制代码
void MpuGetOffset(void) //校准
{
	int32_t buffer[6]={0};
	int16_t i;  
	uint8_t k=30;
	const int8_t MAX_GYRO_QUIET = 5;
	const int8_t MIN_GYRO_QUIET = -5;	
/*           wait for calm down    	                                                          */
	int16_t LastGyro[3] = {0};
	int16_t ErrorGyro[3];	
	/*           set offset initial to zero    		*/
	
	memset(MpuOffset,0,12);
	MpuOffset[2] = 8192;   //set offset from the 8192  
	
	TIM_ITConfig(  //使能或者失能指定的TIM中断
		TIM1,
		TIM_IT_Update ,
		DISABLE  //使能
		);	
	while(k--)//30次静止则判定飞行器处于静止状态
	{
		do
		{
			delay_ms(10);
			MpuGetData();
			for(i=0;i<3;i++)
			{
				ErrorGyro[i] = pMpu[i+3] - LastGyro[i];
				LastGyro[i] = pMpu[i+3];	
			}			
		}while ((ErrorGyro[0] >  MAX_GYRO_QUIET )|| (ErrorGyro[0] < MIN_GYRO_QUIET)//标定静止
					||(ErrorGyro[1] > MAX_GYRO_QUIET )|| (ErrorGyro[1] < MIN_GYRO_QUIET)
					||(ErrorGyro[2] > MAX_GYRO_QUIET )|| (ErrorGyro[2] < MIN_GYRO_QUIET)
						);
	}	

/*           throw first 100  group data and make 256 group average as offset                    */	
	for(i=0;i<356;i++)//水平校准
	{		
		MpuGetData();
		if(100 <= i)//取256组数据进行平均
		{
			uint8_t k;
			for(k=0;k<6;k++)
			{
				buffer[k] += pMpu[k];
			}
		}
	}

	for(i=0;i<6;i++)
	{
		MpuOffset[i] = buffer[i]>>8;
	}
	TIM_ITConfig(  //使能或者失能指定的TIM中断
		TIM1, 
		TIM_IT_Update ,
		ENABLE  //使能
		);
	FLASH_write(MpuOffset,6);//将数据写到FLASH中,一共有6个int16数据
}

三.遥控程序

摇杆ADC采集和转换

配置 4 路 ADC 采集遥控摇杆值。DMA 自动采集,转换完成后自动将 ADC 结果存于ADC_ConvertedValue 。

cpp 复制代码
void ADC1_Mode_Config(void)
{
   DMA_InitTypeDef DMA_InitStructure;
   ADC_InitTypeDef ADC_InitStructure;

/* DMA channel1 configuration */
  DMA_DeInit(DMA1_Channel1);
  DMA_InitStructure.DMA_PeripheralBaseAddr = ADC1_DR_Address; //ADC 结果寄存器地址
  DMA_InitStructure.DMA_MemoryBaseAddr = (u32)&ADC_ConvertedValue;//输入数组地址地址
  DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC;
  DMA_InitStructure.DMA_BufferSize = 4;//转换 4 路
  DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;//外设地址固定
  DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; //内存地址固定
  DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord; //半字(12bit ADC存放)
  DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord;
  DMA_InitStructure.DMA_Mode = DMA_Mode_Circular; //循环传输
  DMA_InitStructure.DMA_Priority = DMA_Priority_High;
  DMA_InitStructure.DMA_M2M = DMA_M2M_Disable;
  DMA_Init(DMA1_Channel1, &DMA_InitStructure);

/* Enable DMA channel1 */
  DMA_Cmd(DMA1_Channel1, ENABLE);

/* ADC1 configuration */
  ADC_InitStructure.ADC_Mode = ADC_Mode_Independent; //独立 ADC 模式
  ADC_InitStructure.ADC_ScanConvMode = ENABLE ; //禁止扫描模式,扫描模式用于多通道采集
  ADC_InitStructure.ADC_ContinuousConvMode = ENABLE; //开启连续转换模式,即不停地进行 ADC 转换
  ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_None; //不使用外部触发转换
  ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right; //采集数据右对齐
  ADC_InitStructure.ADC_NbrOfChannel = 4; //4 路 ADC 通道
  ADC_Init(ADC1, &ADC_InitStructure);

/*配置 ADC 时钟,为 PCLK2 的 8 分频,即 6MHz,ADC 频率最高不能超过 14MHz*/
  RCC_ADCCLKConfig(RCC_PCLK2_Div8); 

/*配置 ADC1 的通道 11 为 55. 5 个采样周期,序列为 1 */ 
  ADC_RegularChannelConfig(ADC1, ADC_Channel_0, 1, ADC_SampleTime_55Cycles5);
  ADC_RegularChannelConfig(ADC1, ADC_Channel_1, 2, ADC_SampleTime_55Cycles5);
  ADC_RegularChannelConfig(ADC1, ADC_Channel_2, 3, ADC_SampleTime_55Cycles5);
  ADC_RegularChannelConfig(ADC1, ADC_Channel_3, 4, ADC_SampleTime_55Cycles5);

/* 使能 DMA 外设*/
  ADC_DMACmd(ADC1, ENABLE);

/*使能 ADC1 外设 */
  ADC_Cmd(ADC1, ENABLE);

/*复位校准寄存器 */ 
  ADC_ResetCalibration(ADC1);

/*等待校准寄存器复位完成 */
  while(ADC_GetResetCalibrationStatus(ADC1));
/* ADC 校准 */
  ADC_StartCalibration(ADC1);
/* 等待校准完成*/
  while(ADC_GetCalibrationStatus(ADC1));
/* 软件启动 ADC 转换 */ 
  ADC_SoftwareStartConvCmd(ADC1, ENABLE);
}

每 10ms 进行一次 ADC 数据的转换为航模遥控数据:

12bitADC(0~4096)*0.25 +1000 ≈ 航模标准数据 1000~2000

相关推荐
智者知已应修善业44 分钟前
【51单片机用数码管显示流水灯的种类是按钮控制数码管加一和流水灯】2022-6-14
c语言·经验分享·笔记·单片机·嵌入式硬件·51单片机
智商偏低7 小时前
单片机之helloworld
单片机·嵌入式硬件
青牛科技-Allen8 小时前
GC3910S:一款高性能双通道直流电机驱动芯片
stm32·单片机·嵌入式硬件·机器人·医疗器械·水泵、
森焱森10 小时前
无人机三轴稳定控制(2)____根据目标俯仰角,实现俯仰稳定化控制,计算出升降舵输出
c语言·单片机·算法·架构·无人机
白鱼不小白10 小时前
stm32 USART串口协议与外设(程序)——江协教程踩坑经验分享
stm32·单片机·嵌入式硬件
S,D11 小时前
MCU引脚的漏电流、灌电流、拉电流区别是什么
驱动开发·stm32·单片机·嵌入式硬件·mcu·物联网·硬件工程
芯岭技术14 小时前
PY32F002A单片机 低成本控制器解决方案,提供多种封装
单片机·嵌入式硬件
youmdt14 小时前
Arduino IDE ESP8266连接0.96寸SSD1306 IIC单色屏显示北京时间
单片机·嵌入式硬件
嘿·嘘15 小时前
第七章 STM32内部FLASH读写
stm32·单片机·嵌入式硬件
Meraki.Zhang15 小时前
【STM32实践篇】:I2C驱动编写
stm32·单片机·iic·驱动·i2c