94国产精品,高清久久久,日韩免费网站

//###############################################
// Author : Gale
// Email : galemx@126.com
// Location: FuZhou FuJian China
//
#include "KK_C.H"
#include "F_DELAY.H"
#include "F_EEROM.H"
//###############################################
// Global Vars define
// 全局變量定義
//--------------------------------------
//##Gyro signal 陀螺儀信號
uchar DevRev; //Gyro rev flags 陀螺儀信號反轉標識
uchar SoftSet; //Fly control software setting 飛控軟件設置
uchar AxisMode; //Fly control mode 飛控模式
//--------------------------------------
//##Arm 鎖定
bool InLock=1; //model is in lock 模型處于鎖定狀態
uchar ArmCnt=0; //Count for arm/disarm
//###############################################
//
// Stick exp
// 搖桿指數調整
//
int StickExp(int stk)
{
uchar neg=(stk<0);
stk*=stk;
stk/=128;
if(neg) stk=-stk;
return stk;
}
//###############################################
//
// Stick angle limit
// 搖桿角度限幅
//
int StickLimitValue(int v)
{
if(v>PPM_MAX) return PPM_MAX;
if(v<0) return 0;
return v;
}
//###############################################
//
// Board initialization
// 系統初始化
//
void Init(void)
{
//Disable all int 禁用全部中斷
CLI();
//Port direction 設置端口方向
DDRB=0x7F; //0b01111111
DDRC=0xC0; //0b11000000
DDRD=0xF1; //0b11110001
//Interrupt setting 設置中斷
PCICR=0x05; //0b00000101 PB7,PB1
PCMSK0=0x80; //0b10000000 PB7
PCMSK2=0x02; //0b00000010 PD1
EICRA=0x05; //0b00000101 PD2,PD3
EIMSK=0x03; //0b00000011 PD2,PD3
//Set timer1 to 1M 將定時器1設為1M
TCCR1B=0x02; // 1/8 sysclk, 1us count
//Set timer2 to 8us count
TCCR2B=4; // 1/64 sysclk,8us count
//Enable interrupts 打開中斷
SEI();
//Delay to avoid power jitter
//延時2秒避開電源不穩定階段
Delay100ms(20);
}
//###############################################
//
// Test arming/disarming
// 判斷是否需要鎖定/解鎖
//----Hold rud stick for a while, ARMING_TIME should mul main-loop cycle
//----保持方向搖桿一會后解/鎖，ARMING_TIME乘以主循環周期就是時間
//
#define ARMING_TIME 250
void ArmingRoutine(void)
{
//Count for anti-jitter 鎖定消抖動
if(RxRud<-STICKGATE || RxRud>STICKGATE) ArmCnt++;
else ArmCnt=0;
//Hold rud stick for a while, the num should mul main-loop cycle
if(ArmCnt>ARMING_TIME)
{
if(InLock)
{
if(RxRud>STICKGATE)
{
GyroBaseCnt=GYROBASECNT;
InLock=0;
}
}
else
{
if(RxRud<-STICKGATE) InLock=1;
}
}
}
//###############################################
//
// Gain scale (return gyro*gain/128)
// 感度調整
//
int GainAdj(int gyro,uchar gain)
{
int r;
r=gyro/8;
r*=gain;
return r/(128/8);
}
//###############################################
//
// Caculate plane attitude
// 計算飛行器姿態
//
void CaclAttitude(void)
{
GyroRead();
//If no gyro base, calibrate gyro
//如果還未建立基準，建立它
if(GyroBaseCnt)
{
GyroBaseRol+=GyroRol;
GyroBaseRol/=2;
GyroBasePit+=GyroPit;
GyroBasePit/=2;
GyroBaseYaw+=GyroYaw;
GyroBaseYaw/=2;
GyroBaseCnt--;
if(!(GyroBaseCnt&7)) LED0_TOG(); //Shine LED show gyro cali 閃爍LED表示在進行陀螺儀校準
GyroRolI=GyroPitI=GyroYawI=0;//Reset I value 清空積分值
}
else
{
if(InLock)
{
//熄滅LED表示在鎖定中
LED0_OFF();
}
else
{
//Remove base part from gyro value
//減去基礎值
GyroRol-=GyroBaseRol;
GyroPit-=GyroBasePit;
GyroYaw-=GyroBaseYaw;
//Reverse gyro signals if necessary
//根據設置反轉各個陀螺儀信號
if(BITTST(DevRev,GYRO_ROL)) GyroRol=-GyroRol;
if(BITTST(DevRev,GYRO_PIT)) GyroPit=-GyroPit;
if(BITTST(DevRev,GYRO_YAW)) GyroYaw=-GyroYaw;
//Gyro feature compensation
//陀螺儀特性補償
GyroRol=GyroCompe(GyroRol,GyroRolPN);
GyroPit=GyroCompe(GyroPit,GyroPitPN);
GyroYaw=GyroCompe(GyroYaw,GyroYawPN);
//Sum integral value with return
//帶回歸計算積分值
GyroRolI=GyroIntegral(GyroRolI,GyroRol);
GyroPitI=GyroIntegral(GyroPitI,GyroPit);
GyroYawI=GyroIntegral(GyroYawI,GyroYaw);
//Light LED
//點亮LED表示在工作中
LED0_ON();
}
}
}
//###############################################
//
// Axis signal mixer
// 計算電機輸出信號
//
void AxisMixer(void)
{
int thr,ail,ele,rud;
//Stick exp
//搖桿指數
if(BITTST(SoftSet,SOFT_EXP))
{
//Rudder do not need exp
thr=StickExp(RxThr);
ail=StickExp(RxAil)/2;
ele=StickExp(RxEle)/2;
}
else
{
thr=RxThr;
ail=RxAil/4;
ele=RxEle/4;
}
//Add gyro to adjustment
//將陀螺儀信號累加到調節量上
ail+=GainAdj(GyroRol,GainRol)+GainAdj(GyroRolI,GainPit);
ele+=GainAdj(GyroPit,GainRol)+GainAdj(GyroPitI,GainPit);
rud=RxRud/4+GainAdj(GyroYaw,GainYaw);//+GainAdj(GyroYawI,GainPit);
if(AxisMode==AXIS_CROSS)
{
// + Mode 十字模式
// 1
// 3 + 2
// 4
Motor1=MotorLimitValue(thr - ele + rud);
Motor2=MotorLimitValue(thr - ail - rud);
Motor3=MotorLimitValue(thr + ail - rud);
Motor4=MotorLimitValue(thr + ele + rud);
}
else
{
// X Mode X模式
// 1 2
// X
// 3 4
Motor1=MotorLimitValue(thr + ail - ele + rud);
Motor2=MotorLimitValue(thr - ail - ele - rud);
Motor3=MotorLimitValue(thr + ail + ele - rud);
Motor4=MotorLimitValue(thr - ail + ele + rud);
}
}
//###############################################
//
// Main routine
// 主程序
//
void main(void)
{
Init(); //Board init 初始化系統
PpmWaitSignal(); //Wait rx signal, led will flash 等待接收機的信號，等待時LED閃爍
Setup(); //Load & Adjust parameters 加載&調節參數
//Main loop 主循環
LED0_OFF();
while(1)
{
TimerRst();
CaclAttitude(); //Caculate plane attitude 計算飛行器姿態
TimerTo(1000);
//Do something between MotorControlBegin() and MotorControlEnd
//See functions declaration, can not exceed 1000us
//在MotorControlBegin()和MotorControlEnd()之間干點事兒，注意不要超過1000us
MotorControlBegin(); //Output head of ppm signal 輸出PPM信號的頭部分
PpmReadSignal(); //Read rx 讀取接收機信號 140us
if(RxThr<RxThrLow) //If thr shutdown 如果油門關閉
{
GyroGainRead(); //Read gain 讀取感度電位器 670us
ArmingRoutine(); //Arm/disarm 加鎖解鎖測試 5us
}
else
{
AxisMixer(); //Cacl motor signal 計算電機信號 305us
}
//If locked(arm) or no gyro base, shutdown all motor
//如果處于鎖定態或者陀螺儀基準未建立，關閉所有馬達
if(InLock || GyroBaseCnt || RxThr<5)
{
Motor1=Motor2=Motor3=Motor4=0;
}
//MOTOR6_L(); //I use it for test execute period 我用來觀察執行時間的
MotorControlEnd(); //Output whole ppm signal 輸出完整的PPM信號
}
}
…………限于本文篇幅余下代碼請從51黑下載附件…………

復制代碼

久久久久久久999_99精品久久精品一区二区爱城_成人欧美一区二区三区在线播放_国产精品日本一区二区不卡视频_国产午夜视频_欧美精品在线观看免费