資源簡介
匿名四軸飛控源碼

代碼片段和文件信息
/********************?(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個文件信息
- 上一篇:CardView+ListView
- 下一篇:飛思卡爾電磁直立完整的程序
評論
共有 條評論