美国十次在线导航,伊人大杳蕉中文在线看免,亚洲人jizzjizz日本人

/******************** (C) COPYRIGHT 2014 ANO Tech ********************************
* 作者：匿名科創
* 文件名：init.c
* 描述：飛控初始化
**********************************************************************************/
#include "include.h"
#include "pwm_out.h"
#include "mpu6050.h"
#include "i2c_soft.h"
#include "led.h"
#include "ctrl.h"
#include "ms5611.h"
#include "ak8975.h"
#include "ultrasonic.h"
u8 All_Init()
{
NVIC_PriorityGroupConfig(NVIC_GROUP); //中斷優先級組別設置
SysTick_Configuration(); //滴答時鐘
I2c_Soft_Init(); //初始化模擬I2C
PWM_IN_Init(); //初始化接收機采集功能
PWM_Out_Init(400); //初始化電調輸出功能
Usb_Hid_Init(); //飛控usb接口的hid初始化
MS5611_Init(); //氣壓計初始化
Delay_ms(400); //延時
MPU6050_Init(20); //加速度計、陀螺儀初始化，配置20hz低通
LED_Init(); //LED功能初始化
Usart2_Init(500000); //串口2初始化，函數參數為波特率
//Usart2_Init(256000);
//TIM_INIT();
Para_Init(); //參數初始化
Delay_ms(100); //延時
Ultrasonic_Init(); //超聲波初始化
Cycle_Time_Init();
ak8975_ok = !(ANO_AK8975_Run());
if( !mpu6050_ok )
{
LED_MPU_Err();
}
else if( !ak8975_ok )
{
LED_Mag_Err();
}
else if( !ms5611_ok )
{
LED_MS5611_Err();
}
return (1);
}
/******************* (C) COPYRIGHT 2014 ANO TECH *****END OF FILE************/

復制代碼

/******************** (C) COPYRIGHT 2014 ANO Tech ********************************
* 作者：匿名科創
* 文件名：rc.c
* 描述：遙控器通道數據處理
**********************************************************************************/
#include "include.h"
#include "rc.h"
#include "mymath.h"
#include "mpu6050.h"
s8 CH_in_Mapping[CH_NUM] = {0,1,2,3,4,5,6,7}; //通道映射
void CH_Mapping_Fun(u16 *in,u16 *Mapped_CH)
{
u8 i;
for( i = 0 ; i < CH_NUM ; i++ )
{
*( Mapped_CH + i ) = *( in + CH_in_Mapping[i] );
}
}
s16 CH[CH_NUM];
float CH_Old[CH_NUM];
float CH_filter[CH_NUM];
float CH_filter_Old[CH_NUM];
float CH_filter_D[CH_NUM];
u8 NS,CH_Error[CH_NUM];
u16 NS_cnt,CLR_CH_Error[CH_NUM];
s16 MAX_CH[CH_NUM] = {1900 ,1900 ,1900 ,1900 ,1900 ,1900 ,1900 ,1900 }; //搖桿最大
s16 MIN_CH[CH_NUM] = {1100 ,1100 ,1100 ,1100 ,1100 ,1100 ,1100 ,1100 }; //搖桿最小
char CH_DIR[CH_NUM] = {0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 }; //搖桿方向
#define CH_OFFSET 500
float filter_A;
void RC_Duty( float T , u16 tmp16_CH[CH_NUM] )
{
u8 i;
s16 CH_TMP[CH_NUM];
static u16 Mapped_CH[CH_NUM];
if( NS == 1 )
{
CH_Mapping_Fun(tmp16_CH,Mapped_CH);
}
else if( NS == 2 )
{
CH_Mapping_Fun(RX_CH,Mapped_CH);
}
for( i = 0;i < CH_NUM ; i++ )
{
if( (u16)Mapped_CH[i] > 2500 || (u16)Mapped_CH[i] < 500 )
{
CH_Error[i]=1;
CLR_CH_Error[i] = 0;
}
else
{
CLR_CH_Error[i]++;
if( CLR_CH_Error[i] > 200 )
{
CLR_CH_Error[i] = 2000;
CH_Error[i] = 0;
}
}
if( NS == 1 || NS == 2 )
{
if( CH_Error[i] ) //單通道數據錯誤
{
}
else
{
//CH_Max_Min_Record();
CH_TMP[i] = ( Mapped_CH[i] ); //映射拷貝數據，大約 1000~2000
if( MAX_CH[i] > MIN_CH[i] )
{
if( !CH_DIR[i] )
{
CH[i] = LIMIT ( (s16)( ( CH_TMP[i] - MIN_CH[i] )/(float)( MAX_CH[i] - MIN_CH[i] ) *1000 - CH_OFFSET ), -500, 500); //歸一化，輸出+-500
}
else
{
CH[i] = - LIMIT ( (s16)( ( CH_TMP[i] - MIN_CH[i] )/(float)( MAX_CH[i] - MIN_CH[i] ) *1000 - CH_OFFSET ), -500, 500); //歸一化，輸出+-500
}
}
else
{
fly_ready = 0;
}
}
}
else //未接接收機或無信號（遙控關閉或丟失信號）
{
}
//=================== filter ===================================
// 全局輸出，CH_filter[],0橫滾，1俯仰，2油門，3航向范圍：+-500
//=================== filter ===================================
filter_A = 3.14f *20 *T;
if( ABS(CH_TMP[i] - CH_filter[i]) <100 )
{
CH_filter[i] += filter_A *(CH[i] - CH_filter[i]) ;
}
else
{
CH_filter[i] += 0.5f *filter_A *( CH[i] - CH_filter[i]) ;
}
// CH_filter[i] = Fli_Tmp;
CH_filter_D[i] = ( CH_filter[i] - CH_filter_Old[i] );
CH_filter_Old[i] = CH_filter[i];
CH_Old[i] = CH[i];
}
//======================================================================
Fly_Ready(T); //解鎖判斷
//======================================================================
if(++NS_cnt>200) // 400ms 未插信號線。
{
NS_cnt = 0;
NS = 0;
}
}
u8 fly_ready = 0;
s16 ready_cnt=0;
void Fly_Ready(float T)
{
if( CH_filter[2] < -400 ) //油門小于10%
{
if( fly_ready && ready_cnt != -1 ) //解鎖完成，且已退出解鎖上鎖過程
{
//ready_cnt += 1000 *T;
}
#if(USE_TOE_IN_UNLOCK)
if( CH_filter[3] < -400 )
{
if( CH_filter[1] > 400 )
{
if( CH_filter[0] > 400 )
{
if( ready_cnt != -1 ) //外八滿足且退出解鎖上鎖過程
{
ready_cnt += 3 *1000 *T;
}
}
}
}
#else
if( CH_filter[3] < -400 ) //左下滿足
{
if( ready_cnt != -1 && fly_ready ) //判斷已經退出解鎖上鎖過程且已經解鎖
{
ready_cnt += 1000 *T;
}
}
else if( CH_filter[3] > 400 ) //右下滿足
{
if( ready_cnt != -1 && !fly_ready ) //判斷已經退出解鎖上鎖過程且已經上鎖
{
ready_cnt += 1000 *T;
}
}
#endif
else if( ready_cnt == -1 ) //4通道(CH[3])回位
{
ready_cnt=0;
}
}
else
{
ready_cnt=0;
}
if( ready_cnt > 1000 ) // 1000ms
{
ready_cnt = -1;
//fly_ready = ( fly_ready==1 ) ? 0 : 1 ;
if( !fly_ready )
{
fly_ready = 1;
mpu6050.Gyro_CALIBRATE = 2;
}
else
{
fly_ready = 0;
}
}
}
void Feed_Rc_Dog(u8 ch_mode) //400ms內必須調用一次
{
NS = ch_mode;
NS_cnt = 0;
}
//=================== filter ===================================
// 全局輸出，CH_filter[],0橫滾，1俯仰，2油門，3航向范圍：+-500
//=================== filter ===================================
u8 height_ctrl_mode = 0;
extern u8 ultra_ok;
void Mode()
{
if( !fly_ready || CH_filter[THR]<-400 ) //只在上鎖時以及油門低于10% 的時候，允許切換模式，否則只能向模式0切換。
{
if( CH_filter[AUX1] < -200 )
{
height_ctrl_mode = 0;
}
else if( CH_filter[AUX1] < 200 )
{
height_ctrl_mode = 1;
}
else
{
if(ultra_ok == 1)
{
height_ctrl_mode = 2;
}
else
{
height_ctrl_mode = 1;
}
}
}
else
{
if( CH_filter[AUX1] < -200 )
{
height_ctrl_mode = 0;
}
}
}

復制代碼

欧美极品高清xxxxhd,国产日产欧美最新,无码AV国产东京热AV无码,国产精品人与动性XXX,国产传媒亚洲综合一区二区,四库影院永久国产精品,毛片免费免费高清视频,福利所导航夜趣136