匿名科创源码

匿名四轴飞控源码

/******************** （C） COPYRIGHT 2014 ANO Tech ********************************
  * 作者   ：匿名科创
 * 文件名  ：ctrl.c
 * 描述    ：飞控控制
 * 官网    ：www.anotc.com
 * 淘宝    ：anotc.taobao.com
 * 技术Q群 ：190169595
**********************************************************************************/

#include “ctrl.h“
#include “height_ctrl.h“

ctrl_t ctrl_1;
ctrl_t ctrl_2;

void Ctrl_Para_Init（）		//设置默认参数
{
//====================================
	ctrl_1.PID[PIDROLL].kdamp  = 1;
	ctrl_1.PID[PIDPITCH].kdamp = 1;
	ctrl_1.PID[PIDYAW].kdamp 	 = 1;
	
	ctrl_1.FB = 0.20;   //外  0}

xyz_f_t except_A = {000};

xyz_f_t ctrl_angle_offset = {000};

xyz_f_t compensation;

void CTRL_2（float T）
{
// 	static xyz_f_t acc_no_g;
// 	static xyz_f_t acc_no_g_lpf;
//=========================== 期望角度 ========================================
	except_A.x  = MAX_CTRL_ANGLE  *（ my_deathzoom（ （ CH_filter[ROL]） 30 ）/500.0f ）;   //30
	except_A.y  = MAX_CTRL_ANGLE  *（ my_deathzoom（ （-CH_filter[PIT]） 30 ）/500.0f ）;  //30
	if（ Thr_Low == 0 ）
	{
		except_A.z += （s16）（ MAX_CTRL_YAW_SPEED *（ my_deathzoom_2（ （CH_filter[YAW]） 40 ）/500.0f ） ） *T ;  //50
	}
	else
	{
		except_A.z += 1 *3.14 *T *（ Yaw - except_A.z ）;
	}
	except_A.z = To_180_degrees（except_A.z）;
//==============================================================================
// 	acc_no_g.x =  mpu6050.Acc.x - reference_v.x *4096;
// 	acc_no_g.y =  mpu6050.Acc.y - reference_v.y *4096;
// 	acc_no_g.z =  mpu6050.Acc.z - reference_v.z *4096;
// 	
// 	acc_no_g_lpf.x += 0.5f *T *3.14f * （ acc_no_g.x - acc_no_g_lpf.x ）;
// 	acc_no_g_lpf.y += 0.5f *T *3.14f * （ acc_no_g.y - acc_no_g_lpf.y ）;
// 	acc_no_g_lpf.z += 0.5f *T *3.14f * （ acc_no_g.z - acc_no_g_lpf.z ）;
// 	
// 	compensation.x = LIMIT（ 0.003f *acc_no_g_lpf.x -1010 ）;
// 	compensation.y = LIMIT（ 0.003f *acc_no_g_lpf.y -1010 ）;
// 	compensation.z = LIMIT（ 0.003f *acc_no_g_lpf.z -1010 ）;
//==============================================================================	

  /* 得到角度误差 */
	ctrl_2.err.x =  To_180_degrees（ ctrl_angle_offset.x + except_A.x - Roll  ）;
	ctrl_2.err.y =  To_180_degrees（ ctrl_angle_offset.y + except_A.y - Pitch ）;
	ctrl_2.err.z =  To_180_degrees（ ctrl_angle_offset.z + except_A.z - Yaw	 ）;
	/* 计算角度误差权重 */
	ctrl_2.err_weight.x = ABS（ctrl_2.err.x）/ANGLE_TO_MAX_AS;
	ctrl_2.err_weight.y = ABS（ctrl_2.err.y）/ANGLE_TO_MAX_AS;
	ctrl_2.err_weight.z = ABS（ctrl_2.err.z）/ANGLE_TO_MAX_AS;
	/* 角度误差微分（跟随误差曲线变化）*/
	ctrl_2.err_d.x = 10 *ctrl_2.PID[PIDROLL].kd  *（ctrl_2.err.x - ctrl_2.err_old.x） *（ 0.005f/T ） *（ 0.65f + 0.35f *ctrl_2.err_weight.x ）;
	ctrl_2.err_d.y = 10 *ctrl_2.PID[PIDPITCH].kd *（ctrl_2.err.y - ctrl_2.err_old.y） *（ 0.005f/T ） *（ 0.65f + 0.35f *ctrl_2.err_weight.y ）;
	ctrl_2.err_d.z = 10 *ctrl_2.PID[PIDYAW].kd 	 *（ctrl_2.err.z - ctrl_2.err_old.z） *（ 0.005f/T ） *（ 0.65f + 0.35f *ctrl_2.err_weight.z ）;
	/* 角度误差积分 */
	ctrl_2.err_i.x += ctrl_2.PID[PIDROLL].ki  *ctrl_2.err.x *T;
	ctrl_2.er

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

    ..A..H.      6148  2016-04-18 17:31  F407_FC_ANO\.DS_Store

     文件      11173  2016-04-01 16:50  F407_FC_ANO\applications\ctrl.c

     文件        697  2016-03-30 20:55  F407_FC_ANO\applications\ctrl.h

     文件      26178  2016-04-12 17:37  F407_FC_ANO\applications\data_transfer.c

     文件       1567  2016-04-12 17:37  F407_FC_ANO\applications\data_transfer.h

     文件       2739  2016-04-01 16:50  F407_FC_ANO\applications\filter.c

     文件        365  2016-03-30 20:55  F407_FC_ANO\applications\filter.h

     文件       7091  2016-04-01 16:50  F407_FC_ANO\applications\height_ctrl.c

     文件        679  2016-03-30 20:55  F407_FC_ANO\applications\height_ctrl.h

     文件       6650  2016-04-01 16:50  F407_FC_ANO\applications\imu.c

     文件        417  2016-03-30 20:55  F407_FC_ANO\applications\imu.h

     文件       3171  2016-04-01 16:50  F407_FC_ANO\applications\include.h

     文件       1672  2016-04-11 18:45  F407_FC_ANO\applications\init.c

     文件        108  2016-03-30 20:55  F407_FC_ANO\applications\init.h

     文件        862  2016-04-01 16:50  F407_FC_ANO\applications\main.c

     文件       8162  2016-04-01 16:50  F407_FC_ANO\applications\mymath.c

     文件       1111  2016-04-01 16:50  F407_FC_ANO\applications\mymath.h

     文件       9852  2016-04-01 16:50  F407_FC_ANO\applications\parameter.c

     文件       1325  2016-04-01 16:50  F407_FC_ANO\applications\parameter.h

     文件       5646  2016-04-01 16:50  F407_FC_ANO\applications\rc.c

     文件        300  2016-04-01 16:50  F407_FC_ANO\applications\rc.h

     文件       3867  2016-04-11 18:45  F407_FC_ANO\applications\scheduler.c

     文件        359  2016-04-01 16:50  F407_FC_ANO\applications\scheduler.h

     文件       3719  2016-03-30 20:55  F407_FC_ANO\applications\stm32f4xx_conf.h

     文件        790  2016-04-01 16:50  F407_FC_ANO\applications\stm32f4xx_it.c

     文件       4069  2016-04-01 16:50  F407_FC_ANO\applications\usbd_user_hid.c

     文件        192  2016-03-30 20:55  F407_FC_ANO\applications\usbd_user_hid.h

     文件      36388  2016-03-30 20:55  F407_FC_ANO\applications\usbd_user_msc.c

     文件      24802  2016-03-30 20:55  F407_FC_ANO\applications\usb_config.c

     文件       1087  2016-03-30 20:55  F407_FC_ANO\applications\version.c

............此处省略446个文件信息