/********************?（C）?COPYRIGHT?2016?ANO?Tech?********************************
??*?作者???：匿名科創
?*?文件名??：anotc_baro_ctrl.c
?*?描述????：氣壓計控制
?*?官網????：www.anotc.com
?*?淘寶????：anotc.taobao.com
?*?技術Q群?：190169595
**********************************************************************************/
#include?“anotc_baro_ctrl.h“
#include?“ms5611.h“
#include?“filter.h“
#include?“fly_mode.h“

float?baro_compensate（float?dTfloat?kupfloat?kdwfloat?vzfloat?lim）
{
	float?z_sin;
	static?float?com_valcom_tar;
	
	z_sin?=?my_sqrt（1-my_pow（vz））;
	
	//com_tar?=?（z_sin/0.44f）?*lim;
	LPF_1_（2.0fdT（（z_sin/0.44f）?*lim）com_tar）;
	com_tar?=?LIMIT（com_tar0lim）;
	
	if（com_val<（com_tar-100））
	{
		com_val?+=?1000?*dT?*kup;
	}
	else?if（com_val>（com_tar+100））
	{
		com_val?-=?1000?*dT?*kdw;
	}
	return?（com_val）;
}

void?fusion_prepare（float?dTfloat?av_arr[]u16?av_numu16?*av_cntfloat?deadzone_height_st?*data_fusion_p_st?*pre_data）
{
	pre_data->dis_deadzone?=?my_deathzoom（data->relative_heightpre_data->dis_deadzonedeadzone）;	
	Moving_Average（av_arrav_num?（av_cnt）（10?*pre_data->dis_deadzone?）&（pre_data->displacement））;?//厘米->毫米
	
//	Moving_Average（av_arrav_num?（av_cnt）（10?*data->relative_height）&（pre_data->dis_deadzone））;?//厘米->毫米	
//	pre_data->displacement?=?my_deathzoom（pre_data->dis_deadzonepre_data->displacement10?*deadzone）;
	
	pre_data->speed?=?safe_div（pre_data->displacement?-?pre_data->displacement_olddT0）;
	pre_data->acceleration?=?safe_div（pre_data->speed?-?pre_data->speed_olddT0）;
	
	
	pre_data->displacement_old?=?pre_data->displacement;
	pre_data->speed_old?=?pre_data->speed;
}

void?acc_fusion（float?dT_f_set_st?*setfloat?est_acc_fusion_p_st?*pre_data_fusion_st?*fusion）
{
	fusion->fusion_acceleration.out?+=?est_acc?-?fusion->est_acc_old;?//估計
	anotc_filter_1（set->b1set->g1dTpre_data->acceleration&（fusion->fusion_acceleration））;??//pre_data->acceleration?//觀測、最優
	
	fusion->fusion_speed_m.out?+=?1.1f?*my_deathzoom（fusion->fusion_acceleration.out020）?*dT;
	anotc_filter_1（set->b2set->g2dTpre_data->speed&（fusion->fusion_speed_m））;
	anotc_filter_1（set->b2set->g2dT（-pre_data->speed?+?fusion->fusion_speed_m.out）&（fusion->fusion_speed_me））;
	fusion->fusion_speed_me.out?=?LIMIT（fusion->fusion_speed_me.out-200200）;
	fusion->fusion_speed_m.a?=?LIMIT（fusion->fusion_speed_m.a-10001000）;
	
	fusion->fusion_displacement.out?+=?1.05f?*（fusion->fusion_speed_m.out?-?fusion->fusion_speed_me.out）?*dT;
	anotc_filter_1（set->b3set->g3dTpre_data->displacement&（fusion->fusion_displacement））;
	
	fusion->est_acc_old?=?est_acc;
}

//超聲波融合參數

#define?SONAR_AV_NUM?50
float?sonar_av_arr[SONAR_AV_NUM];
u16?sonar_av_cnt;

_fusion_p_st?sonar;
_fusion_st?sonar_fusion;
_f_set_st?sonar_f_set?=?{
													0.2f
													0.5f
													0.8f
													
													0.2f
													0.5f
													0.8f	
//													0.2f
//													0.3f
//	

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----

?????文件???60907064??2017-07-30?12:57??匿名領航者飛控源碼、原理圖和上位機\ANO匿名上位機v4.44.exe

?????文件?????204326??2016-07-17?09:57??匿名領航者飛控源碼、原理圖和上位機\原理圖\ANO_Pilot.pdf

?????文件???????6100??2016-08-31?10:59??匿名領航者飛控源碼、原理圖和上位機\飛控源碼\F407_FC_ANO\applications\anotc_baro_ctrl.c

?????文件???????1333??2016-08-31?10:31??匿名領航者飛控源碼、原理圖和上位機\飛控源碼\F407_FC_ANO\applications\anotc_baro_ctrl.h

?????文件???????3697??2017-07-30?12:51??匿名領航者飛控源碼、原理圖和上位機\飛控源碼\F407_FC_ANO\applications\Ano_OF.c

?????文件???????1046??2017-07-30?13:05??匿名領航者飛控源碼、原理圖和上位機\飛控源碼\F407_FC_ANO\applications\Ano_OF.h

?????文件??????12764??2016-12-25?11:30??匿名領航者飛控源碼、原理圖和上位機\飛控源碼\F407_FC_ANO\applications\ctrl.c

?????文件????????752??2016-12-25?11:30??匿名領航者飛控源碼、原理圖和上位機\飛控源碼\F407_FC_ANO\applications\ctrl.h

?????文件??????27386??2017-02-18?20:33??匿名領航者飛控源碼、原理圖和上位機\飛控源碼\F407_FC_ANO\applications\data_transfer.c

?????文件???????1637??2016-09-16?13:13??匿名領航者飛控源碼、原理圖和上位機\飛控源碼\F407_FC_ANO\applications\data_transfer.h

?????文件???????1596??2016-09-16?13:13??匿名領航者飛控源碼、原理圖和上位機\飛控源碼\F407_FC_ANO\applications\data_transfer.h.orig

?????文件???????2625??2016-08-24?12:55??匿名領航者飛控源碼、原理圖和上位機\飛控源碼\F407_FC_ANO\applications\filter.c

?????文件????????646??2016-08-24?08:58??匿名領航者飛控源碼、原理圖和上位機\飛控源碼\F407_FC_ANO\applications\filter.h

?????文件????????909??2016-08-25?10:52??匿名領航者飛控源碼、原理圖和上位機\飛控源碼\F407_FC_ANO\applications\fly_mode.c

?????文件????????275??2016-07-26?15:44??匿名領航者飛控源碼、原理圖和上位機\飛控源碼\F407_FC_ANO\applications\fly_mode.h

?????文件???????8359??2016-08-31?09:39??匿名領航者飛控源碼、原理圖和上位機\飛控源碼\F407_FC_ANO\applications\height_ctrl.c

?????文件????????515??2016-08-24?08:58??匿名領航者飛控源碼、原理圖和上位機\飛控源碼\F407_FC_ANO\applications\height_ctrl.h

?????文件???????7779??2016-08-31?08:52??匿名領航者飛控源碼、原理圖和上位機\飛控源碼\F407_FC_ANO\applications\imu.c

?????文件????????453??2016-08-24?10:03??匿名領航者飛控源碼、原理圖和上位機\飛控源碼\F407_FC_ANO\applications\imu.h

?????文件???????3211??2016-12-25?12:16??匿名領航者飛控源碼、原理圖和上位機\飛控源碼\F407_FC_ANO\applications\include.h

?????文件???????1705??2017-07-30?12:35??匿名領航者飛控源碼、原理圖和上位機\飛控源碼\F407_FC_ANO\applications\init.c

?????文件????????108??2015-06-27?09:28??匿名領航者飛控源碼、原理圖和上位機\飛控源碼\F407_FC_ANO\applications\init.h

?????文件????????862??2016-04-11?20:35??匿名領航者飛控源碼、原理圖和上位機\飛控源碼\F407_FC_ANO\applications\main.c

?????文件???????9377??2016-07-29?09:13??匿名領航者飛控源碼、原理圖和上位機\飛控源碼\F407_FC_ANO\applications\mymath.c

?????文件???????1792??2016-07-27?13:30??匿名領航者飛控源碼、原理圖和上位機\飛控源碼\F407_FC_ANO\applications\mymath.h

?????文件???????9781??2016-08-25?10:15??匿名領航者飛控源碼、原理圖和上位機\飛控源碼\F407_FC_ANO\applications\parameter.c

?????文件???????1325??2016-04-11?20:35??匿名領航者飛控源碼、原理圖和上位機\飛控源碼\F407_FC_ANO\applications\parameter.h

?????文件???????1687??2016-12-25?12:17??匿名領航者飛控源碼、原理圖和上位機\飛控源碼\F407_FC_ANO\applications\PID.c

?????文件???????2095??2016-07-19?13:40??匿名領航者飛控源碼、原理圖和上位機\飛控源碼\F407_FC_ANO\applications\PID.h

?????文件???????6601??2016-08-29?15:08??匿名領航者飛控源碼、原理圖和上位機\飛控源碼\F407_FC_ANO\applications\rc.c

............此處省略626個文件信息