#include?“control.h“
#include?“BSP/BSP.H“
#include?“app/rc/rc.h“
#include?“app/imu/imu.h“

PID?PID_ROLPID_PITPID_YAW;

void?Pid_init（void）;

void?CONTROL（float?rol?float?pit?float?yaw）
{
??u16?moto1=0moto2=0moto3=0moto4=0;
	
	PID_ROL.pout?=?PID_ROL.P?*?rol;
	PID_ROL.dout?=?PID_ROL.D?*?MPU6050_GYRO_LAST.X;
	
	PID_PIT.pout?=?PID_PIT.P?*?pit;
	PID_PIT.dout?=?PID_PIT.D?*?MPU6050_GYRO_LAST.Y;
	
	PID_YAW.dout?=?-PID_YAW.D?*?MPU6050_GYRO_LAST.Z;
	
	PID_ROL.OUT?=?PID_ROL.pout?+?PID_ROL.iout?+?PID_ROL.dout;
	PID_PIT.OUT?=?PID_PIT.pout?+?PID_PIT.iout?+?PID_PIT.dout;
	PID_YAW.OUT?=?PID_YAW.pout?+?PID_YAW.iout?+?PID_YAW.dout;
	if（Rc_Get.THROTTLE>1200）
	{
		moto3?=?Rc_Get.THROTTLE?-?1000?-?PID_ROL.OUT?-?PID_PIT.OUT?+?PID_YAW.OUT;
		moto4?=?Rc_Get.THROTTLE?-?1000?+?PID_ROL.OUT?-?PID_PIT.OUT?-?PID_YAW.OUT;
		moto1?=?Rc_Get.THROTTLE?-?1000?+?PID_ROL.OUT?+?PID_PIT.OUT?+?PID_YAW.OUT;
		moto2?=?Rc_Get.THROTTLE?-?1000?-?PID_ROL.OUT?+?PID_PIT.OUT?-?PID_YAW.OUT;
		
//		moto2?=?Rc_Get.THROTTLE?-?1000?-?PID_ROL.OUT?-?PID_PIT.OUT?+?PID_YAW.OUT;
//		moto1?=?Rc_Get.THROTTLE?-?1000?+?PID_ROL.OUT?-?PID_PIT.OUT?-?PID_YAW.OUT;
//		moto4?=?Rc_Get.THROTTLE?-?1000?+?PID_ROL.OUT?+?PID_PIT.OUT?+?PID_YAW.OUT;
//		moto3?=?Rc_Get.THROTTLE?-?1000?-?PID_ROL.OUT?+?PID_PIT.OUT?-?PID_YAW.OUT;
	}
	else
	{
		moto1?=?0;
		moto2?=?0;
		moto3?=?0;
		moto4?=?0;
	}
	if（ARMED）	Moto_PwmRflash（moto1moto2moto3moto4）;
	else			Moto_PwmRflash（0000）;
}
/*************************************************
Copyright:www.wellmakers.com
Author:路洋
Date:2014-04-25
Description:初始化PID參數
**************************************************/
void?Pid_init（void）
{
	PID_ROL.P?=?12;
	PID_ROL.I?=?0;
	PID_ROL.D?=?0.2;
	
	PID_PIT.P?=?12;
	PID_PIT.I?=?0;
	PID_PIT.D?=?0.2;
	
	PID_YAW.P?=?0;
	PID_YAW.I?=?0;
	PID_YAW.D?=?0.5;
	
	PID_ROL.pout?=?0;
	PID_ROL.iout?=?0;
	PID_ROL.dout?=?0;
	
	PID_PIT.pout?=?0;
	PID_PIT.iout?=?0;
	PID_PIT.dout?=?0;
	
	PID_YAW.pout?=?0;
	PID_YAW.iout?=?0;
	PID_YAW.dout?=?0;
}

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????目錄???????????0??2014-07-29?00:22??飛控stm32程序\
?????目錄???????????0??2014-07-29?00:22??飛控stm32程序\bee_source\
?????文件?????4754657??2014-07-28?23:30??飛控stm32程序\bee_source.zip
?????目錄???????????0??2014-07-29?00:22??飛控stm32程序\bee_source\bee_source\
?????文件???????79282??2014-04-28?14:16??飛控stm32程序\bee_source\bee_source\ANO_FLY.uvgui.Administrator
?????文件??????146506??2014-04-12?21:52??飛控stm32程序\bee_source\bee_source\ANO_FLY.uvgui.JZP
?????文件??????146752??2013-07-21?16:03??飛控stm32程序\bee_source\bee_source\ANO_FLY.uvgui.LG
?????文件??????146099??2013-07-21?11:26??飛控stm32程序\bee_source\bee_source\ANO_FLY.uvgui.SHAO
?????文件???????79639??2014-04-28?14:05??飛控stm32程序\bee_source\bee_source\ANO_FLY.uvgui_Administrator.bak
?????文件??????140791??2014-04-10?22:06??飛控stm32程序\bee_source\bee_source\ANO_FLY.uvgui_JZP.bak
?????文件??????146739??2013-07-21?15:58??飛控stm32程序\bee_source\bee_source\ANO_FLY.uvgui_LG.bak
?????文件??????145690??2013-07-21?11:26??飛控stm32程序\bee_source\bee_source\ANO_FLY.uvgui_SHAO.bak
?????文件???????27477??2014-04-28?14:16??飛控stm32程序\bee_source\bee_source\ANO_FLY.uvopt
?????文件???????22503??2014-04-28?14:16??飛控stm32程序\bee_source\bee_source\ANO_FLY.uvproj
?????文件???????26276??2014-04-28?14:06??飛控stm32程序\bee_source\bee_source\ANO_FLY_ANO_FLY.dep
?????文件???????27478??2014-04-28?14:05??飛控stm32程序\bee_source\bee_source\ANO_FLY_uvopt.bak
?????文件???????22516??2014-04-28?14:05??飛控stm32程序\bee_source\bee_source\ANO_FLY_uvproj.bak
?????目錄???????????0??2014-07-29?00:22??飛控stm32程序\bee_source\bee_source\APP\
?????目錄???????????0??2014-07-29?00:22??飛控stm32程序\bee_source\bee_source\APP\CONTROL\
?????文件????????2052??2014-04-28?13:42??飛控stm32程序\bee_source\bee_source\APP\CONTROL\Control.c
?????文件?????????296??2014-04-06?12:04??飛控stm32程序\bee_source\bee_source\APP\CONTROL\control.h
?????目錄???????????0??2014-07-29?00:22??飛控stm32程序\bee_source\bee_source\APP\IMU\
?????文件????????4313??2014-04-14?10:41??飛控stm32程序\bee_source\bee_source\APP\IMU\IMU.c
?????文件?????????367??2013-08-31?17:32??飛控stm32程序\bee_source\bee_source\APP\IMU\IMU.h
?????目錄???????????0??2014-07-29?00:22??飛控stm32程序\bee_source\bee_source\APP\RC\
?????文件???????11459??2014-04-05?19:41??飛控stm32程序\bee_source\bee_source\APP\RC\Rc.c
?????文件?????????686??2013-08-31?13:15??飛控stm32程序\bee_source\bee_source\APP\RC\Rc.h
?????目錄???????????0??2014-07-29?00:22??飛控stm32程序\bee_source\bee_source\APP\SYS\
?????文件????????2527??2014-04-28?14:15??飛控stm32程序\bee_source\bee_source\APP\SYS\main.c
?????文件????????8605??2014-04-28?13:56??飛控stm32程序\bee_source\bee_source\APP\SYS\stm32f10x_it.c
?????文件????????2084??2013-07-21?11:26??飛控stm32程序\bee_source\bee_source\APP\SYS\stm32f10x_it.h
............此處省略195個文件信息