#include?“control.h“

PID?PID_ROLPID_PITPID_YAWPID_ALTPID_POS;
PID?PID_PID_1PID_PID_2PID_PID_3PID_PID_4PID_PID_5PID_PID_6PID_PID_7PID_PID_8PID_PID_9PID_PID_10PID_PID_11PID_PID_12;

int16_t?getlast_roll=0geilast_pitch=0;
float?rol_i=0pit_i=0yaw_p=0;

vs16?Moto_PWM_1=0Moto_PWM_2=0Moto_PWM_3=0Moto_PWM_4=0Moto_PWM_5=0Moto_PWM_6=0Moto_PWM_7=0Moto_PWM_8=0;

void?Control（T_float_angle?*att_inT_int16_xyz?*gyr_in?T_RC_Data?*rc_in?u8?armed）
{
	T_float_angle?angle;
	angle.rol?=?att_in->rol?+?（rc_in->ROLL-1500）/30;//15
	angle.pit?=?att_in->pit?-?（rc_in->PITCH-1500）/30;//15
	
	rol_i?+=?angle.rol;
	if（rol_i>2000）
	rol_i=2000;
	if（rol_i<-2000）
	rol_i=-2000;

	PID_ROL.pout?=?PID_ROL.P?*?angle.rol/10;
	PID_ROL.dout?=?-PID_ROL.D?*?gyr_in->Y/100;
	PID_ROL.iout?=?PID_ROL.I?*?rol_i/10;

	pit_i?+=?angle.pit;
	if（pit_i>2000）
	pit_i=2000;
	if（pit_i<-2000）
	pit_i=-2000;

	PID_PIT.pout?=?PID_PIT.P?*?angle.pit/10;
	PID_PIT.dout?=?PID_PIT.D?*?gyr_in->X/100;
	PID_PIT.iout?=?PID_PIT.I?*?pit_i/10;
	
	if（rc_in->YAW<1400||rc_in->YAW>1600）
	{gyr_in->Z=gyr_in->Z+（rc_in->YAW-1500）*2;}//
	yaw_p+=gyr_in->Z*0.0609756f*0.002f;//?+（Rc_Get.YAW-1500）*30
	if（yaw_p>20）
		yaw_p=20;
	if（yaw_p<-20）
		yaw_p=-20;


	PID_YAW.pout=PID_YAW.P*yaw_p;
	PID_YAW.dout?=?PID_YAW.D?*?gyr_in->Z/100;				???
	PID_YAW.iout=0;
	
	if（rc_in->THROTTLE<1200）
	{		
		pit_i=0;
		rol_i=0;
		yaw_p=0;
	}

	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_in->THROTTLE>500&&armed）
	{
		Moto_PWM_1?=?rc_in->THROTTLE?-?1000?-?PID_ROL.OUT?-?PID_PIT.OUT?-?PID_YAW.OUT;
		Moto_PWM_2?=?rc_in->THROTTLE?-?1000?+?PID_ROL.OUT?-?PID_PIT.OUT?+?PID_YAW.OUT;
		Moto_PWM_3?=?rc_in->THROTTLE?-?1000?+?PID_ROL.OUT?+?PID_PIT.OUT?-?PID_YAW.OUT;
		Moto_PWM_4?=?rc_in->THROTTLE?-?1000?-?PID_ROL.OUT?+?PID_PIT.OUT?+?PID_YAW.OUT;

	}
	else
	{
		Moto_PWM_1?=?0;
		Moto_PWM_2?=?0;
		Moto_PWM_3?=?0;
		Moto_PWM_4?=?0;
	}
	Moto_PwmRflash（Moto_PWM_1Moto_PWM_2Moto_PWM_3Moto_PWM_4）;
}

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????目錄???????????0??2017-11-19?23:16??STM32微型四軸\
?????目錄???????????0??2017-11-19?23:16??STM32微型四軸\上位機和元件清單\
?????目錄???????????0??2017-11-19?23:16??STM32微型四軸\上位機和元件清單\上位機和元件清單\
?????文件??????334380??2015-10-16?17:08??STM32微型四軸\上位機和元件清單\上位機和元件清單\參數.jpg
?????文件?????4772675??2015-10-16?16:55??STM32微型四軸\上位機和元件清單\上位機和元件清單\上位機.rar
?????文件???????44375??2015-02-05?17:23??STM32微型四軸\上位機和元件清單\上位機和元件清單\元器件清單.pdf
?????目錄???????????0??2017-11-19?23:16??STM32微型四軸\四元數算法和STM32外部時鐘更改后程序設置方法\
?????目錄???????????0??2017-11-19?23:16??STM32微型四軸\四元數算法和STM32外部時鐘更改后程序設置方法\四元數算法和STM32外部時鐘更改后程序設置方法\
?????文件??????344371??2015-02-05?17:23??STM32微型四軸\四元數算法和STM32外部時鐘更改后程序設置方法\四元數算法和STM32外部時鐘更改后程序設置方法\STM32外部時鐘更改后程序設置方法.pdf
?????文件??????542512??2014-10-27?20:54??STM32微型四軸\四元數算法和STM32外部時鐘更改后程序設置方法\四元數算法和STM32外部時鐘更改后程序設置方法\大話多旋翼飛行器--歐拉角與四元數.pdf
?????文件?????7387714??2014-11-05?17:50??STM32微型四軸\四元數算法和STM32外部時鐘更改后程序設置方法\四元數算法和STM32外部時鐘更改后程序設置方法\慣性導航原理教案.pdf
?????目錄???????????0??2017-11-19?23:16??STM32微型四軸\微型四軸程序\
?????目錄???????????0??2017-11-19?23:16??STM32微型四軸\微型四軸程序\微型四軸程序\
?????文件??????172109??2015-10-06?12:20??STM32微型四軸\微型四軸程序\微型四軸程序\ANO_FLY.uvgui.Administrator
?????文件???????78353??2013-11-11?15:09??STM32微型四軸\微型四軸程序\微型四軸程序\ANO_FLY.uvgui.JDBH
?????文件???????74690??2014-04-20?13:13??STM32微型四軸\微型四軸程序\微型四軸程序\ANO_FLY.uvgui.Lenovo
?????文件??????149995??2014-01-04?12:53??STM32微型四軸\微型四軸程序\微型四軸程序\ANO_FLY.uvgui.LG
?????文件??????145336??2014-05-09?17:17??STM32微型四軸\微型四軸程序\微型四軸程序\ANO_FLY.uvgui.WJT
?????文件???????36132??2015-10-06?12:20??STM32微型四軸\微型四軸程序\微型四軸程序\ANO_FLY.uvopt
?????文件???????23904??2015-06-29?19:42??STM32微型四軸\微型四軸程序\微型四軸程序\ANO_FLY.uvproj
?????文件???????57233??2015-10-06?12:16??STM32微型四軸\微型四軸程序\微型四軸程序\ANO_FLY_ANO_FLY.dep
?????目錄???????????0??2017-11-19?23:16??STM32微型四軸\微型四軸程序\微型四軸程序\APP\
?????文件????????2151??2015-07-23?22:29??STM32微型四軸\微型四軸程序\微型四軸程序\APP\Control.c
?????文件?????????564??2014-05-09?15:47??STM32微型四軸\微型四軸程序\微型四軸程序\APP\Control.h
?????文件???????23806??2015-07-23?22:29??STM32微型四軸\微型四軸程序\微型四軸程序\APP\Data_Transfer.c
?????文件?????????886??2015-06-29?19:34??STM32微型四軸\微型四軸程序\微型四軸程序\APP\Data_Transfer.h
?????文件????????3622??2014-03-19?18:56??STM32微型四軸\微型四軸程序\微型四軸程序\APP\Imu.c
?????文件?????????286??2013-11-11?13:27??STM32微型四軸\微型四軸程序\微型四軸程序\APP\Imu.h
?????文件????????5244??2015-07-23?22:30??STM32微型四軸\微型四軸程序\微型四軸程序\APP\main.c
?????文件?????????795??2013-11-12?19:58??STM32微型四軸\微型四軸程序\微型四軸程序\APP\Rc.c
?????文件?????????166??2013-11-12?19:58??STM32微型四軸\微型四軸程序\微型四軸程序\APP\Rc.h
............此處省略113個文件信息