第2章四旋翼飛行器項目設(shè)計

2.2.1主程序模塊

2.相關(guān)代碼

#include<Kalman.h>

#include<Wire.h>

#include<Math.h>

floatfRad2Deg=57.f;〃將弧度轉(zhuǎn)為角度的乘數(shù)

constintMPU=0x68;//MPU-6050的Uc地址

constintnValCnt=7；//一次讀取寄存器的數(shù)量

constintnCalibTimes=1000;//校準(zhǔn)時讀數(shù)的次數(shù)

intcalibData[nValCnt];〃校準(zhǔn)數(shù)據(jù)

unsignedlongnLastTime=0;//上一次讀數(shù)的時間

floatfLastRoll=O.Of;//上一次濾波得至U的Roll角度

floatfLastPitch=O.Of;〃上一次濾波得到的Pitch角度

KalmankalmanRoll;//Roll角濾波器

KalmankalmanPitch;//Pitch角濾波器

voidsetup(){

Serial.begin(9600);//初始化串口，指定波特率

Wire.begin();//初始化Wire庫文件

WriteMPUReg(0x6Bz0);//啟動MPU6050設(shè)備

Calibration();〃執(zhí)行校準(zhǔn)

nLastTime=micros();//記錄當(dāng)前時間

)

voidloop(){

intreadouts[nValCnt];

ReadAccGyr(readouts);//讀出測量值

floatrealVals[7];

Rectify(readouts,realVals);//根據(jù)校準(zhǔn)的偏移量進行糾正

//計算加速度向量的模長，均以g為單位

floatfNorm=sqrt(realVals[0]*realVals[0]+realVals[1]realVals[1]

realVals[2]*realVals[2]);

floatfRoll=GetRoll(realVals,fNorm);〃計算Roll角度

if(realVals[1]>0){

fRoll=-fRoll;

)

floatfPitch=GetPitch(realVals,fNorm);//計算Pitch角度

if(realVals[0]<0){

fPitch=-fPitch;

}

//計算兩次測量的時間間隔dt,以s為單位

unsignedlongnCurTime=micros();

floatdt=(double)(nCurTime-nLastTime)/1000000.0;

//對Roll角度和Pitch角度進行卡爾曼濾波

floatfNewRoll=kalmanRoll.getAngle(fRoll,realVals[4],dt);

floatfNewPitch=kalmanPitch.getAngle(fPitch,realVals[5]zdt);

//根據(jù)濾波值計算角速度

floatfRollRate=(fNewRoll-fLastRoll)/dt;

floatfPitchRate=(fNewPitch-fLastPitch)/dt;

//更新Roll角度和Pitch角度

fLastRoll=fNewRoll;

fLastPitch=fNewPitch;

//更新本次時間

nLastTime=nCurTime;

//向串口打印輸出Roll和Pitch的角度，運行時在Arduino開發(fā)板的串口監(jiān)視器中查看

Serial.print(HRoll:");

Serial.print(fNewRoll);Serial.print(1(*);

nn

Serial.print(fRollRate);Serial.print()z\tPitch:);

Serial.print(fNewPitch);Serial.print('(');

Serial.print(fPitchRate);Serial.print(")\n");

delay(10);

)

//向MPU6050寫入一個字節(jié)的數(shù)據(jù)

//指定寄存器地址與一個字節(jié)的值

voidWriteMPUReg(intnReg,unsignedcharnVal){

Wire.beginTransmission(MPU);

Wire.write(nReg);

Wire.write(nVal);

Wire.endTransmission(true);

)

〃從MPU6050讀出一個字節(jié)的數(shù)據(jù)，指定寄存器地址，返回讀出的值

unsignedcharReadMPUReg(intnReg){

Wire.beginTransmission(MPU);

Wire.write(nReg);

Wire.requestFrom(MPU,1,true);

Wire.endTransmission(true);

returnWire.read();

〃從MPU6050讀出數(shù)據(jù)，保存在指定的數(shù)組中

voidReadAccGyr(int*pVals){

Wire.beginTransmission(MPU);

Wire.write(0x3B);

Wire.requestFrom(MPUZnValCnt*2,true);

Wire.endTransmission(true);

for(longi=0;i<nValCnt;++i){

pVals[i]=Wire.read()<<8IWire.read();

)

)

//對大量讀數(shù)進行統(tǒng)計，校準(zhǔn)平均偏移量

voidCalibration()

(

floatvalSums[7]={0.Ofz0.Of,0.Ofz0.Of,0.Ofz0.Of,0.0};

//先求和

for(inti=0;i<nCalibTimes;++i){

intmpuVals[nValCnt];

ReadAccGyr(mpuVals);

for(intj=0;j<nValCnt;++j){

valSums[j]+=mpuVals[j];

}

}

〃再求平均

for(inti=0;i<nValCnt;++i){

calibData[i]=int(valSums[i]/nCalibTimes);

}

calibData[2]+=16384;//設(shè)芯片Z軸豎直向下，設(shè)定靜態(tài)工作點

)

//算得Roll角度

floatGetRoll(float*pRealValszfloatfNorm){

floatfNormXZ=sqrt(pRealVals[0]*pRealVals[0]+pRealVals[2]*pRealVals[2]);

floatfCos=fNormXZ/fNorm;

returnacos(fCos)*fRad2Deg;

)

//算得Pitch角度

floatGetPitch(float*pRealVals,floatfNorm){

floatfNormYZ=sqrt(pRealVals[1]*pRealVals[1]+pRealVals[2]*pRealVals[2]);

floatfCos=fNormYZ/fNorm;

returnacos(fCos)*fRad2Deg;

)

//對讀數(shù)進行糾正，消除偏移，并轉(zhuǎn)換為物理量

voidRectify(int*pReadoutzfloat*pRealVals){

for(inti=0;i<3;++i){

pRealVals[i]=(float)(pReadoutfi]-calibData[i])/16384.Of;

pRealVals[3]=pReadout[3]/340.Of+36.53;

for(inti=4;i<7;++i){

pRealVals[i]=(float)(pReadout[i]-calibData[i])/131.Of;

)

I

2.2.2PID算法

2.相關(guān)代碼

#include<reg52.h>

typedefunsignedcharuChar8;

typedefunsignedintulntlb;

typedefunsignedlongintulnt32;

typedefstructPID_Value

{

ulnt32liEkVal[3];//差值保存，給定和反饋的差值

uChar8uEkFlag[3];//符號，1對應(yīng)負(fù)數(shù)，0對應(yīng)正數(shù)

uChar8uKP_Coe;〃比例系數(shù)

uChar8uKI_Coe;//積分常數(shù)

uChar8uKD_Coe;〃微分常數(shù)

ulntl6iPriVal;//上一時刻值

ulntl6iSetVal;//設(shè)定值

ulntl6iCurVal;//實際值

}PID_ValueStr;

PID_ValueStrPID;//定義一個結(jié)構(gòu)體，存儲算法中要用到的各種數(shù)據(jù)

bitg_bPIDRunFlag=0;//運行標(biāo)志位，P1D算法不是一直在運算，而是每隔一定時間算一次

/*********************************************************

/*函數(shù)名稱：PID_Operation()

/*函數(shù)功能：P1D運算

/*入口參數(shù)：無(隱形輸入，系數(shù)、設(shè)定值等)

/*出口參數(shù)：無(隱形輸出，U(k))

/*函數(shù)說明：U(k)+KP*[E(k)-E(k-1)]+KI*E(k)+KD*[E(k)-2E(k-1)+E(k-2)]

*********************************************************/

voidPID_Operation(void)

(

ulnt32Temp[3]={0};//中間臨時變量

ulnt32PostSum=0;//正數(shù)和

ulnt32NegSum=0;〃負(fù)數(shù)和

if(PID.iSetVal>PID.iCurVal)〃設(shè)定值是否大于實際值

{

if(PID.iSetVal-PID.iCurVal>10)〃偏差是否大于10

PID.iPriVal=100;//偏差大于10為上限幅值輸出(全速加熱)

else//否則慢慢進行

Temp[0]=PID.iSetVal-PID.iCurVal;//偏差<=10,計算E(k)

PID.uEkFlag[l]=0;//E(k)為正數(shù)，因為設(shè)定值大于實際值

//數(shù)值進行移位，注意順序，否則會覆蓋前面的數(shù)值

PID.liEkVal[2]=PID.liEkVal[1];

PID.liEkVal[1]=PID.liEkVal[0];

PID.liEkVal[0]=Temp[0];

if(PID.liEkVal[0]>PID.liEkVal[1])//E(k)>E(k-1)

(

Temp[0]=PID.liEkVal[0]-PID.liEkVal[1];//E(k)>E(k-1)

PID.uEkFlag[0]=0;//E(k)-E(k-1)為正數(shù)

}

else

(

Temp[0]=PID.liEkVal[1]-PID.liEkVal[0];//E(k)<E(k-1)

PID.uEkFlagfO]=1;//E(k)-E(k-1)為負(fù)數(shù)

)

Temp[2]=PID.liEkVal[1]*2;//2E(k-1)

if((PID.liEkVal[O]+PID.liEkVal[2])>Temp[2])//E(k-2)+E(k)>2E(k-1)

(

Temp[2]=(PID.liEkVal[0]+PID.liEkVal[2])-Temp[2];

PID.uEkFlag[2]=0;//E(k-2)+E(k)-2E(k-1)為正數(shù)

)

else//E(k-2)+E(k)<2E(k-1)

(

Temp[2]=Temp[2]-(PID.liEkVal[0]+PID.liEkVal[2]);

PID.uEkFlag[2]=1;//E(k-2)+E(k)-2E(k-1)為負(fù)數(shù)

}

Temp[0]=(ulnt32)PID.uKP_Coe*Temp[0];//KP*[E(k)-E(k-1)]

Temp[1]=(ulnt32)PID.uKI_Coe*PID.liEkVal[0];//KI*E(k)

Temp[2]=(ulnt32)PID.uKD_Coe*Temp[2];//KD*[E(k-2)+E(k)-2E(k-1)]

〃以下代碼所有的正數(shù)項疊加，負(fù)數(shù)項疊加

/*=========KP*[E(k)-E(k-1)]Wffl=========*/

if(PID.uEkFlag[O]==0)

PostSum+=Temp[0];//正數(shù)和

else

NegSum+=Temp[0];〃負(fù)數(shù)和

/*=========計算K工*E(k)的值=========*/

if(PID.uEkFlag[l]==0)

PostSum+=Temp[1];//正數(shù)和

else

;/*空操作，因為PID.iSetVal>PID.iCurVal(即E(K)>0)才進入if語句的，那么不可能

為負(fù)，所以打個轉(zhuǎn)回去即可*/

/*=========計算KD*[E(k-2)+E(k)-2E(k.-l)]的值=========*/

if(PID.uEkFlag[2]==0)

PostSum+=Temp[2];//正數(shù)和

else

NegSum+=Temp[2];〃負(fù)數(shù)和

/*=========計算U(k)=========*/

PostSum+=(ulnt32)PID.iPriVal;

if(PostSum>NegSum)//是否控制量為正數(shù)

(

Temp[0]=PostSum-NegSum;

if(Temp[0]<100)//小于上限幅值則為計算值輸出

PID.iPriVal=(ulntl6)Temp[0];

elsePID.iPriVal=100;//否則為上限幅值輸出

)

else//控制量輸出為負(fù)數(shù)，則輸出0(下限幅值輸出)

PID.iPriVal=0;

)

)

elsePID.iPriVal=0;//同上

)

2.2.3飛控核心代碼

2.相關(guān)代碼

#include<avr/io.h>

#include"config.h"

#includendef.h"

#include<avr/pgmspace.h>

#defineVERSION220

enumrc{

ROLL,

PITCH,

YAW,

THROTTLE,

AUX1,

AUX2,

AUX3,

AUX4

)；

enumpid{

PIDROLL,

PIDPITCH,

PIDYAW,

PIDALTz

P工DPOS,

PIDPOSR,

PIDNAVR,

PIDLEVEL,

PIDMAG,

PIDVEL,

PIDITEMS

)；

constcharpidnames[]PROGMEM=

"ROLL;n

"PITCH;n

"YAW;"

"ALT;"

"Pos;"

"PosR;n

"NavR;"

"LEVEL;n

"MAG;"

HVEL;"

enumbox{

BOXARM,

#ifACC

BOXANGLE,

BOXHORIZON,

#endif

#ifBARO&&(!defined(SUPPRESS_BARO_ALTHOLD))

BOXBARO,

#endif

#ifdefVARIOMETER

BOXVARIO,

#endif

#ifMAG

BOXMAG,

BOXHEADFREE,

BOXHEADADJ,

#endif

#ifdefined(SERVO_TILT)||defined(GIMBAL)||defined(SERVO_MIX_TILT)

BOXCAMSTAB,

#endif

#ifdefined(CAMTRIG)

BOXCAMTRIG,

#endif

#ifGPS

BOXGPSHOME,

BOXGPSHOLD,

#endif

#ifdefined(FIXEDWING)||defined(HELICOPTER)

BOXPASSTHRU,

#endif

#ifdefined(BUZZER)

BOXBEEPERON,

#endif

#ifdefined(LED_FLASHER)

BOXLEDMAX,//高亮

BOXLEDLOW,//低亮

#endif

#ifdefined(LANDING_LIGHTS_DDR)

BOXLLIGHTS,//啟用任何高度的著陸燈

#endif

#ifdefINFLIGHT_ACC_CALIBRATION

BOXCALIB,

#endif

#ifdefGOVERNOR_P

BOXGOV,

#endif

#ifdefOSD_SWITCH

BOXOSD,

#endif

CHECKBOXITEMS

)；

constcharboxnames[]PROGMEM=//動態(tài)生成配置GU工的名稱

"ARM;"

#ifACC

nANGLE;"

"HORIZON;"

#endif

#ifBARO&&(!defined(SUPPRESS_BARO_ALTHOLD))

"BARO;"

#endif

#ifdefVARIOMETER

"VARIO;"

#endif

#ifMAG

nMAG;"

"HEADFREE;"

"HEADADJ;

#endif

#ifdefined(SERVO_TILT)||defined(GIMBAL)||defined(SERVO_MIX_TILT)

"CAMSTAB;"

#endif

#ifdefined(CAMTRIG)

“CAMTRIG;"

#endif

#ifGPS

”GPSHOME;"

”GPSHOLD;"

#endif

#ifdefined(FIXEDWING)||defined(HELICOPTER)

“PASSTHRU;"

#endif

#ifdefined(BUZZER)

“BEEPER;”

#endif

#ifdefined(LED_FLASHER)

HLEDMAX;n

nLEDLOW;n

#endif

#ifdefined(LANDING_LIGHTS_DDR)

"LLIGHTS;"

#endif

#ifdefINFLIGHT_ACC_CALIBRATION

"CALIB;n

#endif

#ifdefGOVERNOR_P

,rGOVERNOR;"

#endif

#ifdefOSD_SWITCH

”O(jiān)SDSW;n

#endif

7

constuint8_tboxids[]PROGMEM={//永久性ID,依靠工D來識別BOX的功能

0,//ARM模式

#ifACC

1,//ANGLE模式

2,//HOR工ZON模式

#endif

#ifBARO&&(!defined(SUPPRESS_BARO_ALTHOLD))

3,//BARO模式

#endif

#ifdefVARIOMETER

4,〃VAR工。模式

#endif

#ifMAG

5,//MAG模式

6,//HEADFREE模式

7,//HEADADJ模式

#endif

#ifdefined(SERVO_TILT)||defined(GIMBAL)||defined(SERVO_MIX_TILT)

8,//CAMSTAB模式

#endif

#ifdefined(CAMTRIG)

9,〃CAMTR工G模式

#endif

#ifGPS

10,//GPSHOME模式

11,//GPSHOLD模式

#endif

#ifdefined(FIXEDWING)||defined(HELICOPTER)

12z//PASSTHRU模式

#endif

#ifdefined(BUZZER)

13,//BEEPER模式

#endif

#ifdefined(LED_FLASHER)

14,//LEDMAX模式

15,/“LEDLOW模式

#endif

#ifdefined(LANDING_LIGHTS_DDR)

16,〃LL工GHTS模式

#endif

#ifdefINFLIGHT_ACC_CALIBRATION

17,//CAL工B模式

#endif

#ifdefGOVERNOR_P

18,//GOVERNOR模式

#endif

#ifdefOSD_SWITCH

19,//OSD_SWITCH模式

#endif

)；

staticuint32_tcurrentTime=0;

staticuintl6_tpreviousTime=0;

staticuintl6tcycleTime=0;//實現(xiàn)全循環(huán)的微秒數(shù)，并在P工D循環(huán)中使用

staticuintl6_tcalibratingA=0;//校準(zhǔn)在主循環(huán)中完成，每個循環(huán)下降到0后進入正常模

式

staticuintl6_tcalibratingB=0;〃壓力校準(zhǔn)為獲得新的地面壓力值

staticuintl6_tcalibratingG;

staticuintl6_tacc_lG;

staticuintl6tacc_25deg;

staticintl6tgyroADC[3],accADC[3],accSmooth[3],magADC[3];

staticintl6theading,magHold,headFreeModeHold;//[-180;+180]

staticuint8_tvbat;〃電池電壓為0.IV步長

staticuint8tvbatMin=VBATNOMINAL;//最彳氐電》也電壓為0.IV步長

staticuint8trcOptions[CHECKBOXITEMS];

staticint32tBaroAlt,EstAlt,AltHold;//cm為單位

staticintl6_tBaroPID=0;

staticintl6terrorAltitudel=0;

staticintl6tvario=0;//以cm/s單位為變率計

#ifdefined(ARMEDTIMEWARNING)

staticuint32tArmedTimeWarningMicroSeconds=0;

#endif

staticintl6tdebug[4];

staticintl6tsonarAlt;

structflags_struct{

uint8_tOK_TO_ARM:1;

uint8_tARMED:1;

uint8_tI2C_INIT_DONE:1;

uint8_tACC_CAL工BRATED:1

uint8_tNUNCHUKDATA:1;

uint8_tANGLE_MODE:1;

uint8_tHORIZON_MODE:1;

uint8_tMAG_MODE:1;

uint8_tBARO_MODE:1;

uint8_tGPS_HOME_MODE:1;

uint8_tGPS_HOLD_MODE:1;

uint8_tHEADFREE_MODE:1;

uint8_tPASSTHRU_MODE:1;

uint8_tGPS_FIX:1;

uint8_tGPS_FIX_HOME:1;

uint8_tSMALL_ANGLES_25:1

uint8_tCALIBRATE_MAG:1;

uint8_tVARIO_MODE:1;

)f；

#ifdefined(LOG_VALUES)||defined(LCD_TELEMETRY)

staticuintl6_tcycleTimeMax=0;//最大周期時間

staticuintl6_tcycleTimeMin=65535;//最小周期時間

staticuintl6tpowerMax=0;

staticint32tBAROaltMax;

#endif

#ifdefined(LOG_VALUES)||defined(LCD_TELEMETRY)||defined(ARMEDTIMEWARNING)

IIdefined(LOG_PERMANENT)

staticuint32_tarmedTime=0;

#endif

staticintl6_ti2c_errors_count=0;

staticintl6_tannex650_overrun_count=0;

//自動ACC偏移校準(zhǔn)

#ifdefined(INFLIGHT_ACC_CALIBRATION)

staticuintl6_tInflightcalibratingA=0;

staticintl6_tAcclnflightCalibrationArmed;

staticuintl6_tAcclnflightCalibrationMeasurementDone=0;

staticuintl6_tAcclnflightCalibrationSavetoEEProm=0;

staticuintl6_tAcclnflightCalibrationActive=0;

#endif

//功率計

#ifdefined(POWERMETER)

#definePMOTOR_SUM8

staticuint32_tpMeter[PMOTOR_SUM+1];

staticuint8_tpMeterV;

staticuint32_tpAlarm;

staticuintl6_tpowerValue=0;

#endif

staticuintl6_tintPowerMeterSum,intPowerTriggerl;

//遙測

#ifdefined(LCD_TELEMETRY)

staticuint8_ttelemetry=0;

staticuint8_ttelemetry_auto=0;

#endif

#ifdefLCD_TELEMETRY_STEP

staticchartelemetryStepSequence[]=LCD_TELEMETRY_STEP;

staticuint8_ttelemetryStepIndex=0;

#endif

#defineMINCHECK1100

#defineMAXCHECK1900

#defineROL_LO(1?(2*ROLL))

#defineROL_CE(3?(2*ROLL))

#defineROL_HI(2?(2*ROLL))

#definePIT_LO(1?(2*PITCH))

#definePIT_CE(3?(2*PITCH))

#definePIT_HI(2?(2*PITCH))

#defineYAW_LO(1?(2*YAW))

#defineYAWCE(3?(2*YAW))

#defineYAW_HI(2?(2*YAW))

#defineTHRLO(1?(2*THROTTLE))

#defineTHR_CE(3?(2*THROTTLE))

#defineTHRHI(2?(2*THROTTLE))

staticinti6_tfailsafeEvents=0;

volatileintl6_tfailsafeCnt0;

staticintl6_treData[RC_CHANS];

staticinti6_trcCommand[4];

staticinti6_tlookupPitchRollRC[6];

staticintl6_tlookupThrottleRC[11];

staticuintl6_trssi;

#ifdefined(SPEKTRUM)

volatileuint8_tspekFrameFlags;

volatileuint32_tspekTimeLast;

#endif

#ifdefined(OPENLRSv2MULTI)

staticuint8_tpot_P,pot_I;

#endif

//GYRO+ACC工MU模式

staticintl6_tgyroData[3]={0,0,0};

staticintl6_tgyroZero[3]={0,0,0};

staticintl6_tangle[2]={0,0};〃絕對角度傾斜度為0.1°

//陀螺儀+工MU模式

staticintl6_taxisPID[3];

staticintl6_tmotor[NUMBER_MOTOR];

#ifdefined(SERVO)

staticintl6_tservo[8]={1500,1500,1500,1500,1500,1500,1500,1500};

#endif

//EEPROM布局定義

staticuint8_tdynP8[3],dynD8[3];

staticstruct{

uint8_tcurrentset;

intl6_taccZero[3];

intl6_tmagZero[3];

uint8_tchecksum;

}global_conf;

staticstruct{

uint8_tcheckNewConf;

uint8_tP8[PIDITEMS]z18[PIDITEMS]rD8[PIDITEMS];

uint8_trcRate8;

uint8_trcExpo8;

uint8_trollPitchRate;

uint8_tyawRate;

uint8tdynThrPID;

uint8_tthrMid8;

uint8_tthrExpo8;

inti6_tangleTrim[2];

uintl6_tactivate[CHECKBOXITEMS];

uint8_tpowerTriggerl;

#ifdefFLYING_WING

uintl6_twing_left_mid;

uintl6_twing_right_mid;

#endif

#ifdefTRI

uintl6_ttri_yaw_middle;

#endif

#ifdefinedHELICOPTER||defined(AIRPLANE)||defined(SINGLECOPTER)||

defined(DUALCOPTER)

inti6_tservoTrim[8];

#endif

#ifdefined(GYRO_SMOOTHING)

uint8_tSmoothing[3];

#endif

#ifdefined(FAILSAFE)

inti6_tfailsafe_throttle;

#endif

#ifdefVBAT

uint8_tvbatscale;

uint8_tvbatlevel_warnl;

uint8_tvbatlevel_warn2;

uint8_tvbatlevel_crit;

uint8_tno_vbat;

#endif

#ifdefPOWERMETER

uintl6_tpsensornull;

uintl6_tpleveldivsoft;

uintl6_tpleveldiv;

uint8_tpint2ma;

#endif

#ifdefCYCLETIME_FIXATED

uintl6_teyeletime_fixated;

#endif

#ifdefMMGYRO

uint8_tmmgyro;

#endif

#ifdefARMEDTIMEWARNING

uintl6_tarmedtimewarning;

#endif

intl6tminthrottle;

#ifdefGOVERNORP

inti6tgovernor?;

intl6tgovernorD;

int8tgovernorR;

#endif

uint8tchecksum;

}conf;

#ifdefLOGPERMANENT

staticstruct{

uintl6tarm;

uintl6tdisarm;

uintl6tstart;

uint32tarmedtime;

uint32tlifetime;

uintl6tfailsafe;

uintl6ti2c;

uint8_trunning;

uint8tchecksum;

}piog;

#endif

//GPS常用變量

staticint32tGPS_coord[2];

staticint32tGPShome[2];

staticint32tGPS_hold[2];

staticuint8tGPSnumSat;

staticuintl6tGPSdistanceToHome;

staticintl6tGPSdirectionToHome;

staticuintl6tGPSaltitude;

staticuintl6tGPS_speed;//GPS速度單位：cm/s

staticuint8tGPS_update=0;//用二進制區(qū)分GPS位置更新

staticintl6tGPS_angle[2]0,0);//應(yīng)用于GPS校正的角度

staticuintl6tGPS_ground_course0;

staticuint8tGPSPresent=0;//來自GPS系列的校驗

staticuint8tGPSEnable0;

#defineLAT0

#defineLON1

//向北（正）或南（負(fù)）：緯度

//向東（正）或西（負(fù)）：經(jīng)度

staticintl6tnav[2];

staticinti6tnav_rated[2];//將速率控制器添加到導(dǎo)航中以使其更平滑

//默認(rèn)POSHOLD控制增益

#definePOSHOLDP11

#definePOSHOLDI0.0

#definePOSHOLD_IMAX20

#definePOSHOLD_RATE_P2.0

#definePOSHOLD_RATE_I0.08

#definePOSHOLD_RATE_D0.045

#definePOSHOLDRATEIMAX20

〃默認(rèn)導(dǎo)航P工D增益

#defineNAV_P1.4

#defineNAV_I0.20

#defineNAV_D0.08

#defineNAVIMAX20

〃導(dǎo)航模式

#defineNAV_MODE_NONE0

#defineNAV_MODE_POSHOLD1

#defineNAVMODEWP2

staticuint8_tnav_mode=NAV_MODE_NONE;//導(dǎo)航模式

staticuint8_talarmArray[16];//建立數(shù)組

#ifBARO

staticint32_tbaroPressure;

staticint32_tbaroTemperature;

staticint32_tbaroPressureSum;

#endif

voidannexcode(){//每個循環(huán)被排除，如果持續(xù)時間小于650”,則不會干擾控制循環(huán)

staticuint32_tcalibratedAccTime;

uintl6_ttmpztmp2;

uint8_taxis,propl,prop2;

#defineBREAKPOINT1500

//PITCH和ROLL只有動態(tài)P1D調(diào)節(jié)，取決于油門值

if(reData[THROTTLE]<BREAKPOINT){

prop2=100;

}else{

if(reData[THROTTLE]<2000)

(

prop2=100-

(uintl6_t)conf.dynThrPID*(reData[THROTTLE]-BREAKPOINT)/(2000-BREAKPOINT);

}else{

prop2=100-conf.dynThrPID;

}

)

for(axis=0;axis<3;axis++){

tmp=min(abs(reData[axis]-MIDRC),500);

#ifdefined(DEADBAND)

if(tmp>DEADBAND){tmp-=DEADBAND;}

else{tmp=0;}

#endif

if(axis!=2){//ROLL&PITCH

tmp2=tmp/100;

rcCommand[axis]=lookupPitchRollRC[tmp2]+(tmp-tmp2*100)*

(lookupPitchRollRC[tmp2+l]-lookupPitchRollRC[tmp2])/100;

propl=100-(uintl6_t)conf.rollPitchRate*tmp/500;

propl=(uintl6_t)propl*prop2/100;

}else{

rcCommand[axis]=tmp;

propl=100-(uintl6_t)conf.yawRate*tmp/500;

)

dynP8[axis]=(uintl6_t)conf.P8[axis]*propl/100;

dynD8[axis]=(uintl6_t)conf.D8[axis]*propl/100;

if(reData[axis]<MIDRC)rcCommand[axis]=-rcCommand[axis];

)

tmp=constrain(reData[THROTTLE]ZMINCHECK,2000);

tmp=(uint32_t)(tmp-MINCHECK)*1000/(2000-MINCHECK);//[MINCHECK;2000]->

[0;1000]

tmp2=tmp/100;

rcCommand[THROTTLE]=lookupThrottleRC[tmp2]+(tmp-tmp2*100)*

(lookupThrottleRC[tmp2+l]-lookupThrottleRC[tmp2])/100;

if(f.HEADFREE_MODE)

{〃進行優(yōu)化

floatradDiff=(heading-headFreeModeHold)*0.0174533f;//其中P工/180~=

0.0174533

floatcosDiff=cos(radDiff);

floatsinDiff=sin(radDiff);

intl6_trcCommand_PITCH=rcCommand[PITCH]*cosDiff+rcCommand[ROLL]*sinDiff;

rcCommand[ROLL]=rcCommand[ROLL]*cosDiff-rcCommand[PITCH]*sinDiff;

rcCommand[PITCH]=rcCommand_PITCH;

)

#ifdefined(POWERMETER_HARD)

uintl6_tpMeterRaw;

staticuintl6_tpsensorTimer=0;

if(!(++psensorTimer%PSENSORFREQ)){

pMeterRaw=analogRead(PSENSORPIN);

powerValue=(conf.psensornull>pMeterRaw?conf.psensornull-pMeterRaw:

pMeterRaw-conf.psensornull);〃不要使用abs(),誘發(fā)隱式轉(zhuǎn)換為uint和超限

if(powerValue<333){〃只接收合理的值，333是特定值

#ifdefLCD_TELEMETRY

if(powerValue>powerMax)powerMax=powerValue;

#endif

}else{

powerValue=333;

}

pMeter[PMOTOR_SUM]+=(uint32_t)powerValue;

)

#endif

#ifdefined(BUZZER)

#ifdefined(VBAT)

staticuint8_tvbatTimer=0;

staticuint8_tind=0;

uintl6_tvbatRaw=0;

staticuintl6_tvbatRawArray[8];

if(!(++vbatTimer%VBATFREQ)){

vbatRawArray[(ind++)%8]=analogRead(V_BATPIN);

for(uint8_ti=0;i<8;i++)vbatRaw+=vbatRawArray[i];

vbat=(vbatRaw*2)/conf.vbatscale;//結(jié)果是以0.IV步進的

)

#endif

alarmHandlerO;//外部蜂鳴器，處理蜂鳴器事件

#endif

#ifdefined(RX_RSSI)

staticuint8_tsig=0;

uintl6_trssiRaw=0;

staticuintl6_trssiRawArray[8];

rssiRawArray[(sig++)%8]=analogRead(RX_RSSI_PIN);

for(uint8_ti=0;i<8;i++)rssiRaw+=rssiRawArray[i];

rssi=rssiRaw/8;

#endif

if((calibratingA>0&&ACC)||(calibratingG>0)){//校準(zhǔn)

LEDPIN_TOGGLE;

}else{

if(f.ACC_CALIBRATED){LEDPIN_OFF;}

if(f.ARMED){LEDPIN_ON;}

}

#ifdefined(LED_RING)

staticuint32_tLEDTime;

if(currentTime>LEDTime){

LEDTime=currentTime+50000;

i2CLedRingState();

}

#endif

#ifdefined(LED_FLASHER)

auto_switch_led_flasher();

#endif

if(currentTime>calibratedAccTime){

if(!f.SMALL_ANGLES_25){

//多重使用ACC,沒有校準(zhǔn)或太傾斜

f.ACC_CALIBRATED=0;

LEDPIN_TOGGLE;

calibratedAccTime=currentTime+100000;

}else{

f.ACC_CALIBRATED=1;

)

)

#if!(defined(SPEKTRUM)&&defined(PROMINI))

#ifdefined(GPS_PROMINI)

if(GPS_Enable==0){serialCom();}

#else

serialCom();

#endif

#endif

#ifdefined(POWERMETER)

intPowerMeterSum=(pMeter[PMOTOR_SUM]/conf.pleveldiv);

intPowerTriggerl=conf.powerTrigger1*PLEVELSCALE;

#endif

#ifdefLCD_TELEMETRY_AUTO

staticchartelemetryAutoSequence[]=LCD_TELEMETRY_AUTO;

staticuint8_ttelemetryAutoIndex=0;

staticuintl6_ttelemetryAutoTimer=0;

if((telemetry_auto)&&(!(++telemetryAutoTimer%

LCD_TELEMETRY_AUTO_FREQ))){

telemetry=telemetryAutoSequence[++telemetryAutoIndex%

strlen(telemetryAutoSequence)];

LCDclear();//確保清除

)

#endif

#ifdefLCD_TELEMETRY

staticuintl6_ttelemetryTimer=0;

if(!(++telemetryTimer%LCD_TELEMETRY_FREQ)){

#if(LCD_TELEMETRY_DEBUG+0>0)

telemetry=LCD_TELEMETRY_DEBUG;

#endif

if(telemetry)lcd_telemetry();

}

#endif

#ifGPS&defined(GPS_LED_INDICATOR)

staticuint32_tGPSLEDTime;

staticuint8_tblent;

if(currentTime>GPSLEDTime){

if(f.GPS_FIX&&GPS_numSat>=5){

if(++blcnt>2*GPS_numSat)blent=0;

GPSLEDTime=currentTime+150000;

if(blent>=10&&((blcnt%2)==0)){STABLEPIN_ON;}else{STABLEPIN_OFF;}

}else{

if((GPS_update==1)&&!f.GPS_FIX){STABLEPIN_ON;}else{STABLEPIN_OFF;}

blent=0;

)

)

#endif

#ifdefined(LOG_VALUES)&&(LOG_VALUES>=2)

if(cycleTime>cycleTimeMax)cycleTimeMax=cycleTime;

if(cycleTime<cycleTimeMin)cycleTimeMin=cycleTime;

#endif

if(f.ARMED){

#ifdefined(LCD_TELEMETRY)||defined(ARMEDTIMEWARNING)||

defined(LOG_PERMANENT)

armedTime+=(uint32_t)cycleTime;

#endif

#ifdefined(VBAT)

if((vbat>conf.no_vbat)&&(vbat<vbatMin))vbatMin=vbat;

#endif

#ifdefLCD_TELEMETRY

#ifBARO

if((BaroAlt>BAROaltMax))BAROaltMax=BaroAlt;

#endif

#endif

)

)

voidsetup(){

#if!defined(GPS_PROMINI)

SerialOpen(0,SERIAL0_COM_SPEED);

#ifdefined(PROMICRO)

SerialOpen(1,SERIAL1_COM_SPEED);

#endif

#ifdefined(MEGA)

SerialOpen(1,SERIAL1_COM_SPEED);

SerialOpen(2ZSERIAL2_COM_SPEED);

SerialOpen(3,SERIAL3_COM_SPEED);

#endif

#endif

LEDPIN_PINMODE;

POWERPIN_PINMODE;

BUZZERPIN_PINMODE;

STABLEPIN_PINMODE;

POWERPINOFF;

initOutput();

#ifdefMULTIPLE_CONFIGURATION_PROFILES

for(global_conf.currentSet=O;global_conf.currentSet<3;

global_conf.currentSet++){

//檢查所有設(shè)置的完整性

readEEPROMO;

)

#else

global_conf.currentSet=O;

readEEPROM();

#endif

readGlobalSet();

readEEPROM();//加載當(dāng)前設(shè)置數(shù)據(jù)

blinkLED(2,40,global_conf.currentSet+1);

configureReceiver();

#ifdefined(PILOTLAMP)

PL_INIT;

#endif

#ifdefined(OPENLRSv2MULTI)

initOpenLRS();

#endif

initSensors();

#ifdefined(I2C_GPS)||defined(GPS_SERIAL)||defined(GPS_FROM_OSD)

GPS_set_pids();

#endif

previousTime=micros();

#ifdefined(GIMBAL)

calibratingA=512;

#endif

calibratingG=512;

calibratingB=200;

#ifdefined(POWERMETER)

for(uint8_ti=0;i<=PMOTOR_SUM;i++)

pMeter[i]=0;

#endif

#ifdefined(GPS_SERIAL)

GPS_SerialInit();

for(uint8_ti=0;i<=5;i++){

GPS_NewData();

LEDPIN_ON

delay(20);

LEDPIN_OFF

delay(80);

if(!GPS_Present){

SerialEnd(GPS_SERIAL);

SerialOpen(0,SERIALO_CO