四旋翼飞控程序

这是由本人亲自调过的基于STM32程序，用于控制四旋翼的动作

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//上周末参加了北京的四轴沙龙，很有意思的活动。通过活动，我意识到，想让跟很多的人来加入我们这个团队，一起来研究四轴！
//我身边玩四轴的人几乎没有，因此程序的移植和调试都是自己做，有时遇到不懂的，也不方便问。
//现在在EEPW这个论坛里，越来越多的人乐于奉献出自己的代码，这是个好的现象，
//因为只有不断交流，才能取得更大的进步！三个臭皮匠，赛过诸葛亮~ 
//    
//考虑再三，我决定把自己移植调试的代码开源出来，供大家学习。希望大家能一起，来加入这个活动，来加入我们。
//因为咱们的套件都是一样的，硬件上是一致的了，那就只需要研究软件上的算法就行了。交流起来会方便的多！
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//四轴DIY活动详情：http://www.eepw.com.cn/event/action/QuadCopter_DIY/
//
//四轴论坛：http://forum.eepw.com.cn/forum/368/1 
//
//我的四轴DIY进程贴：http://forum.eepw.com.cn/thread/248747/1
//
//淘宝店铺：http://item.taobao.com/item.htm?spm=a230r.1.14.23.sYD4gY&id=35605621244
//
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//	BY:让四轴飞，2014430
//
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
#include “control.h“
#include “BSP.H“
#include “rc.h“
#include “imu.h“

PID PID_ROLPID_PITPID_YAW;
u8 ARMED = 0;
extern vs16 QHZYXZ;

float Get_MxMi（float numfloat maxfloat min）
{
	if（num>max）
		return max;
	else if（num		return min;
	else
		return num;
}

void CONTROL（float rol_now float pit_now float yaw_now float rol_tar float pit_tar float yaw_tar）	
{						//当前姿态角，，，目标姿态角
	u16 moto1=0moto2=0moto3=0moto4=0;
	vs16 throttle;
	
	float rol = rol_tar + rol_now;
	float pit = pit_tar + pit_now;
	float yaw = yaw_tar + yaw_now;
	
	throttle = Rc_Get.THROTTLE - 1000;	//1000<遥控油门值<2000
	if（throttle<0）	throttle=0;
	
	PID_ROL.IMAX = throttle/10;			//积分限幅，积分值不超过当前油门值的一半
	PID_ROL.IMAX = Get_MxMi（PID_ROL.IMAX10000）;	//限制积分结果为，0到1000	
	PID_PIT.IMAX = PID_ROL.IMAX;
	
	PID_ROL.pout = PID_ROL.P * rol;
	PID_PIT.pout = PID_PIT.P * pit;
//////////////////、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、	
	/////////////0.1  0.1   30  30
	if（rol_tar*rol_tar<1 && pit_tar*pit_tar<1 && rol_now*rol_now<100 && pit_now*pit_now<100 && throttle>300）
	{			//防止角度大了，积分超调//目标姿态角水平，姿态角几乎水平，油门值不太低
		PID_ROL.iout += PID_ROL.I * rol;
		PID_PIT.iout += PID_PIT.I * pit;
		PID_ROL.iout = Get_MxMi（PID_ROL.ioutPID_ROL.IMAX-PID_ROL.IMAX）;		//对输出的积分限幅
		PID_PIT.iout = Get_MxMi（PID_PIT.ioutPID_PIT.IMAX-PID_PIT.IMAX）;
	}
	else if（throttle<200）
	{			//油门值较小时，积分项清零
		PID_ROL.iout = 0;
		PID_PIT.iout = 0;
	}
//////////////////、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、	

//	rc_roll_d = rol_tar - getlast_roll;
//	getlast_roll = rol_tar;
//	PID_ROL.dout = PID_ROL.D * （MPU6050_GYRO_LAST.X+rc_roll_d*300）;//角速度+控制误差微分
//	
//	rc_pitch_d = pit_tar - getlast_pitch;
//	getlast_pitch = pit_tar;
//	PID_PIT.dout = PID_PIT.D * （MPU6050_GYRO_LAST.Y+rc_pitch_d*300）;//角速度+控制误差微分
	
	PID_ROL.dout = PID_ROL.D * MPU6050_GYRO_LAST.X;		
	PID_PIT.dout = PID_PIT.D * MPU6050_GYRO_LAST.Y;
//////////

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2014-05-22 21:30  FeiKong\
     文件       76877  2014-05-22 21:30  FeiKong\ANO_FLY.uvgui.Administrator
     文件      146671  2013-08-11 16:29  FeiKong\ANO_FLY.uvgui.LG
     文件      146099  2013-07-21 11:26  FeiKong\ANO_FLY.uvgui.SHAO
     文件      140411  2014-04-30 16:24  FeiKong\ANO_FLY.uvgui.Yang
     文件       73765  2014-05-03 21:27  FeiKong\ANO_FLY.uvgui_Administrator.bak
     文件       31262  2014-05-22 21:30  FeiKong\ANO_FLY.uvopt
     文件       22986  2014-05-22 21:30  FeiKong\ANO_FLY.uvproj
     文件       52252  2014-05-22 21:20  FeiKong\ANO_FLY_ANO_FLY.dep
     目录           0  2014-04-02 17:03  FeiKong\APP\
     目录           0  2014-05-22 21:20  FeiKong\APP\CONTROL\
     文件        5767  2014-05-22 21:20  FeiKong\APP\CONTROL\Control.c
     文件         302  2013-08-09 20:04  FeiKong\APP\CONTROL\Control.h
     目录           0  2014-05-22 21:19  FeiKong\APP\I2C_6050\
     文件        1451  2014-03-16 02:09  FeiKong\APP\I2C_6050\delay.c
     文件         333  2013-06-04 11:33  FeiKong\APP\I2C_6050\delay.h
     文件       11413  2014-05-22 21:19  FeiKong\APP\I2C_6050\I2C_MPU6050.c
     文件        2980  2014-04-28 00:45  FeiKong\APP\I2C_6050\I2C_MPU6050.h
     目录           0  2014-05-22 21:20  FeiKong\APP\IMU\
     文件        5877  2014-05-22 21:20  FeiKong\APP\IMU\IMU.c
     文件         355  2013-08-09 21:26  FeiKong\APP\IMU\IMU.h
     目录           0  2014-05-22 21:20  FeiKong\APP\RC\
     文件       10375  2014-05-22 21:20  FeiKong\APP\RC\Rc.c
     文件         595  2013-08-08 19:57  FeiKong\APP\RC\rc.h
     目录           0  2014-05-22 21:18  FeiKong\APP\SYS\
     文件        3267  2014-05-22 21:18  FeiKong\APP\SYS\main.c
     文件        8944  2014-05-22 21:18  FeiKong\APP\SYS\stm32f10x_it.c
     文件        2084  2013-07-21 11:26  FeiKong\APP\SYS\stm32f10x_it.h
     目录           0  2014-05-22 21:20  FeiKong\APP\UART\
     文件        1389  2014-05-22 21:20  FeiKong\APP\UART\Uart1.c
     文件         200  2014-05-22 21:19  FeiKong\APP\UART\uart1.h
............此处省略356个文件信息