《the design of an autopilot controller for an unmanned aerial vehicle》由会员上传分享,免费在线阅读,更多相关内容在学术论文-天天文库。
北京化工大学硕士学位论文TheDesignofanAutopilotControllerforanUnmannedAerialVehicle姓名:AmahahJustice申请学位级别:硕士专业:计算机应用技术指导教师:何宾20090610 THEDESIGNoFANAUToPILoTCoNTRoLLERFoEANUNM睑NNEDAERIALVEHCLEABSTRACTThisthesisdescribesthedesign,implementationandtestingofafullyautonomousandprogrammableautopilotsystemforsmallscaleautonomousunmannedaerialvehicle.Allaspectsoftheautopilotdesignareconsidered,frommodelingtoflighttests.Amathematicalaircraftmodel,asafunctionoftheaircraft’Sphysicalparametersaloneisalsopresented.Controllerarchitecturecapableofregulatingthemotionvariablesrequiredforconventionalflightusinglowcost,off-the—shelfsensorsisdeveloped.Thecontrollerdesigncomplementstheaircraftmodeldevelopmenttechniqueused,byreducingthesensitivityofthecontrollerperformancetothemodelaccuracy.ThecontrollerisacustomPCBwithanembeddedprocessor(ATmega168)combinedwithcircuitrytoswitchcontrolbetweentheRCcontrolandtheautopilotcontr01.Itcontrolsnavigationandaltitudebycontrollingtherudderandthrottle.Itusesflightstabilizationsystem,asensorpack,GlobalpositioningSystem(GPS)andanRFtransceivertomonitorandreportcrucialparameterssuchasaltitude,speed,pitch,rollandposition.Anembeddedsoftwarealgorithmhasbeendevelopedtoenabletheaerialvehicleaccomplishtherequiredautonomyandmaintain 星业!竖望型!里堕21g垒!业型旦!塾212型坚呈!坚:!:垡壁i!satisfactoryflightoperation.Thedesignofaerialvideosystemisalsooutlined,theaerialvideosystemincludesacamerawhichislinkedtoa1000mwirelesstransceiverandagroundbasedreceiver.Resultsfromseveraldaysofflighttestsdemonstratethedesignsuccess.Therapidsuccesscanlargelybeattributedtoextensivesimulationsoftheentireautopilot.Thesefeaturesallowtheautopilottobeoperatedbyawidespectrumofskilledandunskilledusers.Innovativecontroltechniquesimplementedinsoftware,coupledwithlightweight,robust,andinexpensivehardwarecomponentswereusedinthedesignoftheautopilot.KeyWords:UAV,MUX,Autopilot,GPS,FMACo—Pilot,GCSU 北京化工大学位论文原创性声明本人郑重声明:所呈交的学位论文,是本人在导师的指导下,独立进行研究工作所取得的成果。除文中已经注明引用的内容外,本论文不含任何其他个人或集体己经发表或撰写过的作品成果。对本文的研究做出重要贡献的个人和集体,均已在文中以明确方式标明。本人完全意识到本声明的法律结果由本人承担。作者签名:.扭!堕Q日期:关于论文使用授权的说明学位论文作者完全了解北京化工大学有关保留和使用学位论文的规定,即:研究生在校攻读学位期间论文工作的知识产权单位属北京化工大学。学校有权保留并向国家有关部门或机构送交论文的复印件和磁盘,允许学位论文被查阅和借阅;学校可以公布学位论文的全部或部分内容,可以允许采用影印、缩印或其它复制手段保存、汇编学位论文。保密论文注释:本学位论文属于保密范围,在上年解密后适用本授权书。非保密论文注释:本学位论文不属于保密范围,适用本授权书。作者签名:.舳压‰一u水位导师签名:日期:坠掣竺!£呈!殳皇::日期:12:!区尘 ChapterIIntroductionandOverviewI.IBackgroundUA、厂isanacronymthatstandsforUnmannedAcrial喊cle.TheUnmannedAerialⅥ出icleisbasicallyannaircraftt11athasnopilotonboard.UAVScanbemanuallycontrolledortheCanflyautonomously.FIyingautonomouslyinvolvestheuseofanautopilotandwaypointsthathavebeenpre-programmed.Unmannedaerialvehicleshavebeenusedforvariousmissions,aerialmapping,reconnaissance,real-timevideoandcombatmissions.Unmannedaerialvehiclescanbebroadlycategorizedintomanybroaddivisionsbasedonrole,shapeandpurpose.UnmannedaerialvehiclesCanbedividedintocivilianandmilitaryU斟S。TheV斟Scanbedeployedforthefollowingfunctions:1.)Remotesensing:TheremotesensingfunctionsoftheUAVincludeelectromagneticsensors,biologicalSensorsandchemicalsensors.2)Transport:UAVsCallbeusedastransportvehicle;itCanbeusedtoconveygoodsfromonepointtotheother.Mostpayloadsarestoredinaninternalpayloadbaysomewhereintheairframe.3)Scientificresearch:UA、,SCanbeusedforresearchpurposes.MostscientificdatacouldbetoodangeroustocollecthenceaU斟Canbeusedtocollectsuchd钺钆TheNationalOceanicandAtmosphericAdmmistrationhasbeenusinganunmannedaerialvehicleasahurricanehunter.4)Precisionstrikes:ThemajorfunctionofamilitaryUA~厂isitsuseforprecisionstrikes.UnmannedaerialvehicleCanbearmedwithmissilesforcombatmissions.TheMQ-1predatorUAVSarmedwinlHellfiremissilesarenowused嬲platformsforhittinggroundtargetsinsensitiveareas.5)Searchandrescue:UAVSc锄beusedinsearchandr豁t,-qleoperations.Forexample,Predatorsoperating18,000—29,000feetabovesealevel,performedsearchandrescueanddamageassessment.PayloadscarriedwereanopticalSenSor.1.2ADVfUqCESINAUToPILoTCoNTRoLDESIGNFoRUAVUnmannedaerialvehiclesandautopilotcontrollerforthemhavebeenresearchedanddevelopedforseveralyears.MilitaryUAVSarethemostadvancedinthisformofresearch.ThegovernmentiSwillingtofundtheresearchanddevelopment.HoweverthellSeofUAVSinnon.militaryapplicationsiscurrentlylimitedforreasonsofcost.safety,andspecialrequirements.CurrentlytherearenofullyautonomousoperationalUA、厂Savailablewithwingspanslessthan2feettocarryanautopilotandpayload.BclewiSabriefdescriptionofadvancesinthisfield: BeijingUniversityofChemicalTechnologyMaster’sThesis1)In1999theNavalPostgraduateSchoolcompletedthedevelopmentofarapidtestprototypingsystembytheHardware--In·-LoopsimulationmethodforaprototypeU酬namedFrog.Inthisexperimentitwasfoundthatthelateralroot-mean-squareorrorwas21.41feetwhilethelongitudinalrmserrorwas14.949fcet.2)In2001theDepartmentofAeronauticsandAeronauticsinStanfordUniversitydesignedandbuiltaplatformfortheDragonFlyUAV.TheDragonFlyUAV、撕廿lallhardwaremaybepluggedintotheplatform.Furthermore,theplatformCanvalidatetheautopilotaccuracyoftheDragonfly,simulatetheeffectsof1inearandnonlinearcontrollers011theattitudecontrolofUAV,andshowthechangesofthemrCraflstatesandcontrolvariablesatdiversesamplingrates.3)TheHelios,whichiscurrentlyunderdevelopment(USA),willincorporateafuelcellenergystoragesystemtoprovidepowerforflyingthroughthenigllt’makingitcapableofcontinuousflightat50,000—70,000feetformonthsatatime.WithilltenyearstheHeliosU斟isexpectedtobewidelydeployedasbroadbandcommunicationsplatforms,providingauniquelycosteffectivecomplementtosatelliteandterrestrialcommunicationsystems.Ashasbeenshownabove,therehasbeenSOmuchworkandresearchdoneinthedevelopmentQ鼍U划S。themostadvancedtechniquesbeingusedinmilitaryapplicationsalone.ItisworthytonotehoweverthatcurrentU划SarebulkyandinconvenienttotransportandSillallermodelsbelongtoagroupknown嬲Microaerialvehicles.1.3oBJECTIVE/TASKDESCRIPTIoNThisthesispresentsthedevelopment,modelingandimplementationofallautopilottllatcanbeusedinasmallscaledUAVforflightpurposes。Thisthesiswillpresentthefeaturesoftheautopilotthatmakesitusefulforcivilianpurposes.Indevelopingthisautopilot,afewgoalsweresetandaccomplished,belowiSacategorizationofsuchgoalsordesignmetrics:a)Size:ThemostimportantdesignmetricconsideredWassize,thegoaliStobuildasmallscaleautopilotwhichcanfitintoasmallscaleairframefortheUAⅥb)Robust:Theautopilotshouldberobustanddamageresistant.c)Autonomous:OneveryvitaldesignmetricisdesigninganautopilotunitforaU斛thatCanbeautonomous.d)Cost:Thetotaldevelopmentprocessshouldbeinexpensive,shouldbeabletousecornmercialavailable0ff-the.shelfhardware.e)TheUAVshouldbecapableofautomaticlaunchandlanding.TlliSthesiswillshowthedevelopmentofanautopilotsystemthatsatisfiestheabovekeyconsiderationsordesignmetrics.Anoff-the-shelfairfl'alnewaSusedintheautopilotdevelopmenttomaximizetheeffortandresourcestowardthedevelopmentoftheautopilotitSel£2 1.4TECHNICALCHALLENGESManychallengesweremetinthedesignoftheautopilot.Firstofa11wasthechallengeofcreatingadesignthatwillfitthegoalsetinthebeginningoftheproject,功eobjectiveofdesigningasmallscaleUAVautopilotcontroller,andthechallengeofkeepingthisdesigninexpensive.PursuingthedesignofasmallscaleUAVwillinvolveiustabilitiesintheairframeandhencetheautopilotmustbeabletocontrolallunstableairframe.SmallscaleUAVSareinherentlyunstablebecauseofmeirsizeandweightcategory.Thethirdchallengeliesintestinganddevelopinganautopilotonaninexpensiveairframewithoutcrashing01"damagingthehardware.TllisWasimplementedbyusingaHobbicoSuperstarairframe.ItWasmodifiedtomeetthespecificationofthisdesign.Finally,forasmallscaleUAVt0beofcommercialvalue,itmustboastalleaseofuse.Thushi.ghlevelalgorithmsweredevelopedthatsenabletheUAVt0beautonomousandhaveauto.1aunchandlandingcapabilities.1.5THESISOUTLINE,nlcthesisstructureisshownbelow:Chapter2discussesF1i曲tDynamics,definestheaxissystemsusedinthisproject,italSOfocusesonControlalgorithmsusedtostabilizeandnavigatetheUAⅥChapter3willaddresstheAutopilotoverview;itdescribesinthedepththecompleteautopilotsystemanditscomponents.Itgoesaheadtodescribetheindividualcomponents.Chapter4describesthearduinosoftwarewhichisthecoresoftwareusedinthedesignanddevelopmentoftheautopilot,itfurthergoestodescribeasoftwaresimulationfortheautopilot.Chapter5presentstheflighttestsandresultsobtainedandacomparativeanalysisoftheactualflightandtheoreticalexpectations.FinallyconcludesthisthesiswithrecommendationsforfutureworkonU、斛Sandautopilots.3D动”◇ Chapter2FLIGHTDYNAMICSInthischapter,Newton’Slawsareusedtodevelopequationsofmotionsforafixedwingaircraft.TheequationsofmotionofanaircraftisthebasicsonwhichthewholeframeworkofflightdynamicsisbuiltonanditproⅥdestheessentialkeytoaproperunderstandingofn姐ngandhandlingquantities.Attheirsimplest,theequationsofmotionCandescribesrnallperturbationsmotionabouttlletrimonly.AttheirmostcomplextheyCancompletelydescribetheembodyingstaticstability,dynamicstability,aeroelectriceffects,atmosphericdisturbancesandcontrolsystemdynamicsimultaneouslyforagivenaircraftconfiguration.Anattitudedescriptionsystemisthenpresentedandthekinematicequationsaredevelopedandaddedtotheequationsofmotiontoformacompletesetofnon.1inearaircraftdynamicsequatiOns.Finally,theforcesandmomentsthatdrivemeseequationsarewrittenintermsoftheaircraft’Sstatesandphysicalproperties.Thedynamicequationswillbediscussedindetailheresincetheyformthebasisofthesimulationmodelusedlateron.2.1CooRDINATESYSTEMSPriortousingNewton’Slawstodevelopadynamicmodelfortheaircraft,acoordinatesystemmustbedefined.InthiscoordinatesystemallofNewton’SlawsCanbeapplied.111epositionofanaircraftisgenerallydefinedrelativetooneof3setsofco-ordinatesystems:1)WindAxesa)XAxis-Positiveinthedirectionoftheoncomingair(relativewind)b)YAxis—PositivetoRightofXAxis,perpendiculartoXAxisc)ZAxis—Positivedownwards,perpendiculartoX-Yplane.2)InertialAxes(orBodyAxes)曲XAxis.Positiveforward,throughthenoseoftheaircraft.b)YAxis—PositivetotherightofXAxis,perpendiculartoXAxisC)ZAxis—Positivedownwards,perpendiculartoXAxis3)EarthAxesa)XAxis—Positiveinthenorthdirection.b)YAxis·Positiveintheeastdirectionc’lZAxis.Positivetowardsthec圮ntreoftheearthForflightdynamicsapplicationstheEarthaxesaregenerallyofminimaluse,andhencewillbeignored.Themotionsrelevanttodynamicstabilityareusuallytooshortindurationforthemotionoftheearthitselftobeconsideredrelevanttotheaircraft.IIlflightdynamics,pitch,rollandyawanglesmeasureboththeabsoluteattitudeanglesandchangesintheattitudeangles,relativetotheequilibriumorientationoftheaircraft.5 BeijingUniversityofChemicalTechnologyMaster’sThesis1Pitch:ThisiStheangleofXBodyAxisrelativetothehodzon.21Roll:ThisiStheangleofYBodyAxis(wing)relativetothehorizon.3)Yaw:ThisistheangleofXBodyAxis(nose)relativetotheNorth.AlsoapositiverotationabouttheZBodyAxis.Inanalyzingflightdynamicswealeconcernedbomwithrotationandtranslationofthisaxissetwithrespecttoafixedinertialframe.ForallpracticalpurposesalocalEarthaxissetisused,thishasXandYaxisinthelocalhorizontalplane,usuallywimthexaxiscoincidingwiththeprojectionofthevelocityvectoratthestartofthemotion,ontothisplane.Thezaxisisvertical,pointinggenerallytowardstheEarth’scentre,completinganorthogonalset.ThebodyorientationCanbedefinedbythreeEulerangles,theTait.Bryanrotations,aquatemion,oradirectioncosinematrix.Arotationmatrixisparticularlyconvenientforconvertingvelocity,force,angularvelocity,andtorquevectorsbetweenbodyandEarthCOOrdinateframes.2.2DYNAMICMoDELoFUAVThefirststepindesigningacontrollerforanyphysicalsystemistocharacterizethedynamicsofthatsystem.HereweshallstartthederivationfromNewton’Ssecondlawofmotion.Newton’Ssecondlaw:r1,=ma(2.1)Thisisavectorequationthatrelatesthemagnitudeanddirectionofthevector,tothemagnitudeofdirectionoftheaccelerationvector.ThisexpressioncallnowbeusedtoproducetheequationsofmotionexpressedinCartesiancoordinates.EQUATIONSOFMOTIONINCARTESIANCooRDINATESInrectangularCartesiancoordinates,xyz,then:F=F。七F。七xlWAnda=a.+a.+a.xzYJztc(2-3)Theequations(2.2)and(2.3)callnowbeputincomponentformasshownbelow:1r1/-,=ma=rn觉x=黝y=缈y‘6(2-2)俞少 ————一一...£!!呈里堕!!.一一一.—一1r’,=ma=mz■’●●Z(2-4)Thissystemofordinarydifferentialequationsmustbeaugmentedwithappropriateinitialconditions.Typicallywewillknowtheinitialpositionr0andv0.Thus厂(O)=x(Oy+少(O)歹+z(O)尼=厂O1,(O)=戈(Oy+夕(O)/+三(O)尼=y0(2.5)Ingeneralforcesonabodydependingeneralonitspositionr’itsvelocityVand,sometimes,explicitlyOllthetimet(controlinputs011anaircraR).Inaddition,theymayalsodependonthepositionandvelocityofotherneighboringbodies.TheseequationsCanbeintegratedindependentlytocomputethecomponentsofthevelocityvector,匕∽毫㈣+去童≯以o∽=。@+去三y以匕∽叫啪三弦)巩(2-6)(2-7)(2-8)NUMERICALI烈’I’EGRAI.IONThemostgeneralwayofsolvingtheequationsofmotionisbynumericalintegration.Itistypicallyusefultoworkonlywithfirstorderequationsasbelow:戈=v多=y三=yX。YZ(2-9)匕==—m!-u万1,(jf,少,k,、,Y,匕,f)xZo=≯㈨,k儿Y匕,力ymvxZ7(3·O)(3-1) 匕一m!F7(五少,Vx,≯,匕,f)‘,vzThisisasetof6firstorderODE’swithinitialconditions.(3-2)ThefollowingaretheequationsofmotionintheCartesiancoordinates.LongitudinalEquationswritteninthestatefonn:|.五五0=f互乙码\:l一鼍≮t&1‘鼍强xe{芝热|艺墨-|恤‰‰‰豳i虬+妣畋+缎酬【0o‘-|‘Whilethelateralequationsarealsogiveninthestate内肋嬲well:群h.c0S蛾)雕--V-0L0010毒orlpf|三重三。也Ⅳ,l震|十|Ⅳ;Ⅳ二l西lln1)X:inertialXcoordinate(metersnorth)2)YinertialYcoordinate(meterseast)3)H:altitudeininertialcoordinates(meters)4)U:bodyflameX—velocity(meters/second)5)v:bodyframeY-velocity(meters/seconm6)w:bodyflamez-velocity(metefs/secol五)7):rollangle(radians)8)口:pitchangle(radians)9)9:headingangle(radians)10)P:rollrate(radians/seeond)11)Q:pitchrate(radians/second)12)R:yawrate(radians/secondsl81隆].j(3-3)(3—4) Table2.1:Parametersforthecoefficientsoftheequationsofmotion.Mass(slug)0.2369018Liftslope4.08115Pitchstability.0.422680Sideforcetothesideslip.0.107990Rollduetosideslip.0.0524900YawstabilityO.0213716Liftduetopitchrate4.90714Pitchdamping.1.68929Sideforceduetorollrate.0.0385900Rolldamping一0.425470Yawduetorollrate.0.0008823l3Sideforceduetoyawrate.0595832RollduetoyawrateO.0442800Yawdamping一0.0122373Liflduetoelevator0.491440Pitchduetoelevator.0.210653Sideforceduetorudder一0.534090RDllduetorudder.0.438776Yawduetorudder-0.328740SidefcIteeduetoaileronO.0204900Rollduetoaileron0.133270Yawduetoaileron-0.00725753Deformationdrag0.02Dragduetodevondeflection0.04Dragduetoelevondeflection0.04Tobuildacontrollerfortheaircraft,theaircraftsdynamicsmustbemodeled.Thedynamiccharacteristicsofinterestaretheaircraft’Sresponsetostepinputsinthelateralandlongitudinalmodes.TounderstandtheresponseoftheHobbicoSuperstar,amodeloftheHobbicoSuperstarwasimportedintosimulationenvironmentconstructedinMATLABSimulink.2.3SIMULATIoNENVIRONM[ENTThelongitudinalandlateralaircraftdynamicequationswereimportedintoMATLAB/Simulink.TheaircraftcoefficientsfortheHobbicowereobtainedfromSlopeSoaringSimulator.TheSlopeSoaringSimulatorisanopen—sourceflight9 BeijingUniversityofChemicalTechnologyMaster'sThesissimulatorthathasmanydifferentaircraftmodels.TheMATLAB/SimulinkdiagramoftheaircraftdynamicmodelisshowninFigure2.1.Theinputstothemodelarethecontrolsurfacedeflections(5).ThemodelHobbicohasthefollowingcontrolsurfaces:aileron,elevator,throttleandtherudder.Figure2.1:6DOFMATLABAircraftSimulationWhere】【_(X;YH;U;VW:Phi:Theta;Psi;P;Q;R)whichrepresentsthe12statesoftheaircraft.2.4SETTINGUPTHESIMULATIoNThesimulationissetuptocalculatethetrimconditionsforlevel,an.acceleratedflightoftheaircraftmodelatthebeginningofeachsimulation.Thecontroldeflectionsarecenteredaroundthesetrimconditions.Thetrimfunctioncalculatestherequiredpitchangle,andcorrespondingelevatorandthrottledeflections,forthetrimvelocityofthesimulation.Thetrimvelocityusedinmost12m/s,whichisthenominalvelocityoftheHobbicoairfl'ame.ThelateralmodesofinterestaretheDutchroll,spiral,androll.therollandspiralmodesaleofspecialinterestasmeyrepresenttheaircraft’Snaturalabilitytolevelitselfintherollaxisinresponsetochangingcontrolinputorwindgust.ThesemodesareeasilyexcitedbyapplyingastepinputtotheMicron.Toexcitethelateralmodes,theaircraftmodelistrimmedforalevelflightataspeedof12m/s.AstepinputiSappliedtotheaileron.耽eailerondeflectionisneutralizedafter1second.Theaircraft’sresponsein,妒,andParenotedinFigure2.2.Theaileronst印inducesarollrate,whichdampensoutoverthe40secondsimulation.聃erollangleoscillates.andthendampensoutatavalueofabout35degrees.ThesimulationshowsthattheHobbicodevelopsadivergentspiraltosmallstepsinaileron.Thespiraldivergesuntiltherollanglereachesapproximately35degrees.,nlerollanglestabilizesatthispoint.TlliSisconsistentwithflightdatafromtheHobbico.10 Chapter2O10加∞柏∞7BFigure2.2:LateralresponseintheHobbicotostepinaileronThenextsimulationisatestofthelateralresponsetoastepintheruddercontr01.Thepurposeofthissimulationistoshowtheresponseoftheaircrafttoadisturbanceforce.Therudderinputinducesasideslipmotion,whichdampensoutwithin20seconds.Therudderinputalsoinducesarollangle.■■乏a●口5{5O51QO舅耄 BeijingUniversityofChemicalTechnologyMaster'sThesis3垂2善10Stepresponse:Ph;cSol耐),Rudder(dashed)051015加253035404532-耄,0一O510152D253D35404SFigure2.3:Lateralresponse.Thelongitudinalmodeofinterestistheshortperiodmode.Thelongitudinaldynamicsareexcitedinthesamemannerasthelateraldynamics.Withtheaircraftinalevelflighttrimcondition.theelevatorisdeflectedforlsecondandthenreturnedtoneutral.TheresponseoftheaircraftisshowninFigure2.4.Thestepinelevatorinducesapitchratewhichdoesnotdampenoutwithinthe40secondsimulation.ThiSbehaviorisconsistentwithanunstableshortperiodmode.Itisclearthattheaircraftwillrequireactivestabilizationinthepitchaxis.12谤m五}J雾口 1∞芒,50董。瑚10皇£’跚印甩Sp喊HI等嘲^匕目H馐姆苒r嘲J=一、--一一一一一一一一一一●一●一一一一●一●_--__一一一-一一一一一一一一一一一一一一一一一一一一-I-I105'O15∞历∞衢4050O51015∞笛∞35404550Figure2.4:Longitudinalresponse.2.5SIMULATIoNANALYSISThesimulationsshowthattheaircraftexhibitsinstabilitiesinthelongitudinalandlateralmodes.Instabilitiesinthelateralmodesshowthattheaircraftisunstableinrollandtendstodivergetoarollangleof35de蓼eesinsmalllateraldisturbances.Thelongitudinalsimulationsshowtheaircrafttobeunstableintheshortperiodmode.Toaddresstheseinstabilities,theaircraftwillrequirestabilizationinthepitchandrollaXeS.2.6CONTRoLDESIGNIntheprevioussectionitwasdeterminedthattheHobbicohaslongitudinaland13 BeijingUniversityofChemicalTechnologyMaster'sThesis1ateralinstabilities.Inordertoachievestableautonomousflight.theseinstabilitiesmustbeaddressed.ThissectiondiscussesthePIDcontrolstructuredevelopedtostabilizeandcontroltheaircraft.Toflytheaircraftautonomously,theautopilotmustbecapableofnavigatingwaypoints.Tllisrequiresthattheautopilotbeabletocontroltheheading,altitude,andairspeedoftheaircraft.Formanualcontrol,itisdesirabletllattheautopilotsalSOacc印tpitchandrollanglecommands.Toaccomplishthis,acontrollerconstructedofnestedPIDloopshasbeendeveloped.Theaileron,elevator,andthethrottlecommandsarecontrolledviainnerPIDloopsthatstabilizetheroll.pitchandthrottle.Thealtitudeandheadingarecontrolledwithouter100ps.whichproducecommandedvaluesfortheinnerloops.Theautopilotcontrolisdividedintotwocontrollers:thelateralcontrollerandthelongitudinalcontroller.TlliSsectionwilldetaileachofthesecontrollers.2.6.1LATERAI。CoNTRoLThelateralcontrollerisresponsibleforcontrollingtheyawrate,rollangleandheading.TmSisaccomplishedwiththreeinnerservoloopsandoneouterloop.Theinnerloopsproduceeffortsthatdrivetheaileronandredder.Theouterloopsproducecommandedvaluesfortheinnerloops.111einnerlateralloopsareasfollows:1)AileronfromRoll:ThiSloopgeneratesallailerondeflectionfromtherollerror.Thisloopisresponsibleforholdingtherollattitudeoftheaircraft.2.)AileronfromRollRate:T11iSloopgeneratesallailerondeflectionfromtherollrate.Itisresponsiblefordampingtherollrateoftheaircraft.ThecontroleffortforthisloopissummedwiththeeffortfromtheaileronfromtheR01lloopandsenttotheaileronservoactuator.3)RudderfromYawRate:11坞purposeofthisloopistocontrolyawrateoftheaircraft.TIliSloopdrivestheredderservo.111eta-D:-kppitchrate.厂⋯Ilsatblockl一^‘·1。——⋯PitchEstimate(theta)pilotelevatorinputfrom.——一bypass—。一—。—,丫’⑧◇⋯▲Est勋im¨ate-kpUft(Phi)compensation◆羔~-◆。j?【I.~u”JL,、14l卜satblock◆elevatoractuator一{;|囝~R(i呻 Figure2.5:Innerlateralrollandrollratecontroller.YGaywmR。aRte,}_KpYawmte◆bs眈atk}j⋯⋯⋯—JPilotRudderinput—satblock2.6:Innerlateralyawratecontroller.◆RudderactuatorTheouterlateralcontrolloopisthefollowing:1)RollfromHeading:Thisistheloopresponsibleforcontrollingtheheadingofthemrcrafi.Itgeneratesarollanglefromtheheadingerror.ThisrollangleselvesasthecommandedrollanglefortheAileronfromRollloop.Thisloopisshownin2.7。。_H⋯⋯⋯⋯H。_一“!Desired;headingHeadingestimate◆kp+ki/s+kd(s)_bsIocatkl⋯⋯⋯一j2.7:Outerlateralh∞dmganglecontroller.2.6.2LoNGITUDINALCoNTRoL卜desiredroll(PhiD)Thelongitudinalcontrollerisresponsibleforcontrollingthevelocity,pitchangle,andaltitude.Thisisaccomplishedwi也3innerse]rvoloopsand2outerloops.Theinnerloopsproduceeffortsthatdrivetheelevatorandthrottle.Theouterloopsproducecommandedvaluesfortheinnerloops.15丫oH BeijingUniversityofChemicalTechnologyMaster’sThesisThetaDDeslredAirspeedi⋯⋯⋯⋯1譬撩te卜Kp眦hratepbS淼▲⋯-’Kp+Ki/sPitChIl卜PitchEstimatesatblockPitchElevatorinput,◇◇Roll⋯⋯~l—KpliftEstimate,compensation|▲-sat:block+’sat·blockFigure2.8:InnerIon#mdinalpitchandpitchlatecontroller.IqeasuredAirspeedI。Kp+KI/s+Kd(s)I■sat:⋯~。block一圆⋯⋯一Elevatoractuatorsat一一l-ThrottleblockActuatorFigure2.9:InnerIon#mdinalairspeedcontroller.Theouterlateralcontrolloopsareasfollows:1)PitchfromAltitude:ⅢSloopgeneratesacommandedpitchanglesfromthealtitudeerror.TheoutputofthisloopconnectsdirectlytotheelevatorfromPitchloop.,nliSloopiSidealforcontrollingtheaircraftsaltitudewhenthealtitudeerrorissmalJ.21PitchfromAirspeed:眦SloopcontrolstheaircraR’Sairspeedbyadjustingthepitchangle.TheoutputofthisloopconnectsdirectlytotheElevatorfromPitchloop.TIliSloopiSusedtoregulatetheaircraft’Sairspeedduringclimbanddescent.HeasuredAItitude(H)⋯-卜Kp+Ki/s+Kd(s)⋯一-卜satblockFigure2.10:OuterlongitudinalAltitudecontroller.◆DesiredPitch印o▲_{;薹㈣D Airspeed(VpD)Measured(Vp)◆Kp+Ki/s+Kd(s)-卜sat{⋯⋯◆satblock{block:一⋯一一⋯JFigure2.11:Outerlayerlongitudinalairspeedcontroller.2.7CLoSED.LooPAUToPILoTSIMULATIoNTheautopilotcontrolstructureisimplementedinN队TLAB/Simulinkforsimulation.Theautopilotblockisconnectedtotheaircraftmodeldiscussedearlier.TlliSsimulationissetupsuchthatindividualcontrolloopscanbesimulatedseparately.Theinputtothesimulationisthedesiredvelocity,altitude,andheading.Theautopilotperformanceinthelateralandlongitudinalmodesissimulatedbyapplyingstepinputs.Thesimulationoftheclosed-loopautopilotperformanceinthelateralmodes.17o▲卜 BeijingUniversityofChemicalTechnologyMaster’sThesisFigure2.12:Simulinkdiagramofthedosedloopautopilotsimulation.2.8LATERALAUToPILoTSIMULATIoNRoutineTheclosedlooplateralperformanc2oftheautopilotandaircraftissimulatedintwostages.First,theinnerrollPIDloopissimulatedbygivingastepinputinthedesiredrollangle().Theouterheadingloopissimulatedbygivingastepinputinthedesiredheading(力.ThesimulationshowsthatadequateperformanceinthelateralmodesCanadequatelybeachieved、柝thPIDcont 40O-10Step酗P哺:P哺删XPhid为函斟X出lshed》05'O'5∞嚣∞Figure2.14:Closedlooplateralresponsetostepindesiredrollangle.19 呈!女堕墨望!壁!!!堂堂三坚竺竖苎!里!!旦!!!墅竺!壁!!:!!!曼!!!一..-●__--●-●----_____-_●_●_-_●____-__-●●●●———__—__^_________-______________-_--—______————————————————————————一一:1。茎。.10StepirIHeadl叼:P‰《sol硇》,Phid镐l陀磷出塔hed)圆-30Step衲8eadi嗍:Psi岱dⅪl|Psic黯i地d(蠕h印J{020∞柏鞠∞7a∞Figure2。15:Closedlooplateral唧115etOstepindesiredheading.2.9LONGITUDlNALAUToPILoTSIMULATIONTheclosedlooplongitudinalperformanceoftheautopilotandaircraftissimulatedinthreestages.FirsttheinnerpitchPIDloopissimulatedbygivingastepinputinthedesiredpitchangle.Theresponseofthealtitudeloopissimulatednextbygivingastepinthedesiredaltitude(H).Theresponseofthevelocityloopissimulatedinthesamemanner.ThelongJ【tudinalsimulationshowsthatthePIDstructureoutlinedinthischapterCanadequatelycontrolthealtitudeoftheaircraft,vdocity,andpitchangleinresponsetostepinputs.伯5O占加惦●■,X—罨 Figure2.16:Closedlooplongitudinalresponsetostepindesiredpitchangle善15051015抽25∞354045∞secon出2l,曩-t曹譬芒a■o BeijingUniversityofChemicalTechnologyMaster’SThesis1210Figure2.17:ClosedlooplongitudinalresponseOlO∞弧2∞墨{啪拍.640.4OlO∞柏弱∞secon出Figure2.18:Closedlooplongitudinalresponsetostepindesiredvelocity(Vp)CoNCLUSIoNThischapterhasexplainedthedesignofautopilotalgorithmsusedtocontroltheHobbicoSuperstar.Itbeganbyanalyzingthedynamicsofthefixedwingbyapplyingst印inputstoa6-DOFMATLABmodel.T11iSmodelhadlateralandlongitudinalinstabilitiesthathavetobecorrectedwiththeautopilotcontrol100ps.Throughsimulation,itwasfoundthatcontrollercouldadequatelycontrolofthepitch,roll.velocity,altitudeandtheheadingoftheaircraft.Thechapterconcludedbysimulating Chapter2theclosed—loopperformanceoftheautopilotandaircraftbyapplyingstepinputs. Chapter3THEDESIGNOFTHEAUTOPILOTHARDWAREThischapterprc5entsadiscussiononthehardwareaspectoftheautopilotcontrollerdcsignThediscussionwillbeginbyoutliningtheautopilotonasystemlevelandthentheaerialvideosystemwhichwillbeknowasthepayloadwillalsobeshown3.1AI砌rIUMEThopurposeoftheairframeortheflightplatformistocarrytheelectroniccircuitandthepayload(camera)TheflightplatformconsistsoftheairframeandallhardwareneededtokeepthealrfrmeinthemLSpecifically,theflight#reformincludestheairfrolnc,actuators,electronicspeedcontrol,flightbaaeryandpropulsionsystemmperformanceoftheU“syslemdependsheavilyontheflightplatformThechosenflightplatformexhibitsmanyofthedesiredcharacteristiesofsmailscaleunma,nnedaerialvehiclesA900dUAVfli曲‘platformmuststayairbornelongenoughtoCOmpleteuse血Imission,beabletobeportable.becapableofcarryingtheautopilotandpayloadandberesistanttodamagecausedbycrashesandhardlandingsThebattery,moorandcomponentselectionsalsoheavilyinfluencetheperformanceoftheflightplatformWingspan485inWingarea400sqlnWingloading161oT/sqRLengm36llnWeight311bsRadio4-channelwiththreesel'vo$MotorBattery84volt1700mAh.3000mAh BeijingUniversityofChemicalTechnologyMaster'sThesisTable2:HobbicoSpecifications.3.2AUToPILoTSYSTEMTheAutopilotsystemcomprisesofanArduinocompatibleautopilotboard,1HzEM406GPSmodulefromglobalsatandallinfraredflightstabilizationsystern(FMAco-pilot).TheEM。406AGPSmodulebasedonSiRFisbuiltonthesametechnologyasET-301,butalSOincludeson—boardRAM。andabuilt.inpatchantenna.TheinfraredflightstabilizationunitisusedtoSellsethedifferenceininfraredtemperaturebetweenthesky.Theskyisalwaysatarelativelywarmertemperature.Co—pilotusestwopairsofinfraredsensors:onepairpointforeandaft,andtheaircraft’Sorientationrelativetotheearth’Sinfraredhorizon.AileronservoFigure3.2:AutopilotSystemDiagram————一’C0mmand—————■———_+——-————◆Senso体FMSControllerVehicleFigure3=3ConceptualdataflowoftheUAV Chapter33.3CoMPoNENTSoFTHEAUToPILoTSYSTEMTheautopilotsystemcomprisesofvariouscomponents,hereIshallpresentallthecomponentsandanintroductiontothecomponentanditsfunctionintheautopilotsystem.3.3.1TheAutopilotBoardTheautopilotboardarguablyiSthemostimportantandcomplexpieceofhardwareintheU斟system.Thefunctionoftheautopilotistocontroltheaircraflusingtheaircraftsstates.Theautopilotisacompactunitthatcontainsanembeddedprocessor(ATmega168)combinedvyitllcircuitrytoswitchbetweenradiocontrolandautopilotcontrol(that’Sthemultiplexer).TheAtmega168controlsnavigation(followingGPSwaypoints)andaltitudebycontrollingtherudderandthrottle.ThecomponentsusedhereareallCOTSandopensource.TheautopilotisfullyprogrammableandCanhaveanynumberofGPSwaypoints(includingaltitude)andtriggercamerafunctionsaswellasothersensors.Thefeaturesoftheautopilotareasfollows:1Fullyautonomous21Builtinhardwarefailsafethatusesaseparatecircuitrytotransfercontrolfrommanualcontroltoautopilot.3’IIthastheabilitytorebootthemainprocessorinmid-flight41Ithasmultiplewaypoints51Ithasaninterfaceforthe1HzEM406GPSmoduleTheATmega168iaa10w-powerCMOS8.bitmicrocontroUerbasedontheAⅥ己enhancedRISCarchitecture.TlliSisthecentralprocessingunit.itisresponsibleforprocessingsensordata,handlingI/ototheGPSmodule,andit’Sbasicallytheheartoftheautopilot.Byexecutingpowerfulinstructionsunasingleclockcycle,theATmega168achievesthroughputsapproaching1MIPSperMHzallowingthesystemtooptimizepowerconsumptionversusprocessingspeed·TheAVRcorecombinesarichinstructionsetwitll32generalpurposeworkingregistets.IthassufficientRAMandflashmemoryforautopilotSourcecodestorageandruntimeexecution.3.3.2FMAINFRAREDSTABILIZATIoNUNITTheinfraredstabilizationunitisacommercialproduct.Theinfraredstabilizationunithastwounits.ThesensorunitmountsontheoutsideoftheaircraftwhereitCanhaveagoodviewinfourdirections.ItcontainsinfraredthermalSensorsthatcandetecttheaircraftsorientationtothehorizon.Itcalltellwherethehofizonisbased0frtheheatdifferenceinthegroundfromthesky.ThesensorunitistetheredtoacontrolunitthatiSinsidethefuselage.Thisotherunitisconnectedtotheelevatorandaileronserves.Ifanimbalanceisdetectedintheorientationoftheaircraft.thecontrolunit 塑91竖竺!!鉴!墅堕!!兰堕竺堕!!墼!!!!!型!竺堡!:苎!!墅堡deflectsthepropercontrolsurfacetomaintainstability.Inthiswaytheinfraredstabilizationunitisabletomaintainstabilityinpitchandroll.ThestabilizationunitallowsforstraightandlevelflightoftheUAV.3.3.3ACTUAToRSActuatorsasmentionedinthisdesignaretheservosthatweTeusedintheimplementationofthedesign.Belowisalistofservosused:1)Aileronservo:眦sservoistheactuatorfortheaileron.Itistheservousedtocontroltheaileron2)Elevatorscrvo:Tllisservoistheactuatorfortheelevator.Itisusedtocontroltheelevatog3)Rudderservo"TKsservoistheactuatorusedfortheservo.ItspurposeiStocontrolthemovementsoftherudder.TheSa'VOusedistheHitecHS-425BBServomotor.Ithasdualballbearings.impactresingearsandhighperformancecircuitry.Belowarcitsspecifications:1)Speed:0.16secondsat60degrees2)Torque:4。1kegcln一56.93oz/in3)Size:40.6}19.8*36.6mm4)Weight:45.59-1.60zThescrvocontroliscontrolleddirectlybytheembeddedprocessor(ATmega168)。TheprocessorCancommand8servosatthesametimewithRS.232serialport.PWMsignalisusedextensivelyonDCscrvocontrol,suchasthehobbymodelDCservo.AsquarewaveiSoutputted50timespersecond.Thewidthofthesquarewavedecidesthehornoftheservooscillatingan西e’andthewavewidthisdescribedaccordingtothecontinuoustime.WhG强lthewidthofthesquarewaveequals1.5millisecond.thehornoftheservokeepsonneutralposition,45degreeangle.,nlewidthofsquarewillchangefromlt02millisecond,andthehomofservo、机llrotateamount0.90degreeangle.Figure3.4:ServocontrolPWMS翻rvoal-'15"SOfro敏+45。臼国臼 3.3.4GLOBALPoSITIoNINGSYSTEMTheGPSreceiverusedontheautopilotistheEM.406AGPS.TlliSreceiVersuitstheautopilotapplicationforseveralreasons.First,ithasanintegratedantennawhichmakesitself-containedandlow.maintenance.Second.itisverysmallandlightweight.Thirdithasdesirablecharacteristicswhichinclude:l1SiRFstarIIIhighperformancechipset.2)Ithasveryhigllsensitivity(TrackingSensitivity:-19bm)31IthasextremelyfastTTFF(timetofirstfix)atlowsignallevel.4、ItsupportsNMEA0183dataprotoc01.51Ithasabuilt—inSuperCaptoreservesystemdataforrapidsatelliteacquisition.61Ithasabuilt-inpatchantenna7)ItusesLEDindicatOrstoshowGPSlockornotlock.Theposition,heading,velocity,andaltitudeareupdatedonceasecond.Becauseoftheakcraflslowvelocity,thisissn伍cientformostofthcontrolalgorithms.TheautopilotcontrolcouldbeimprovedifafilterwereusedtoprovideapositionandheadingestimatebetweenGPSsamples.3.3.5PAYLoADTheautopilotisdesignedtoaccommodateandinterfacewithawidevarietyofpayloads.TodemonstratetheeffectivenessoftheU斟asanobservationplatfowin.avideosurveillancepayloadisusedfortesting.Thevideosystemconsistsoftheairbomecameraandtransmitter,andtheground.basedreceiVerandvideodisplay.Thediscussionofthevideosystemcomponentsisdividedintotwosections.Thefirstcoverstheairbomecomponents.Thesecondistheground.basedequipmenttoreceiveanddisplaythevideo.Thefirstcomponentoftheairbomesysternisthec冒tmera.ThePanasonicGP.CXl71hi.resolutionCCDcameraisusedbecauseofitShighresolutionandlowweight.Thesecondcomponentisthevideotransmitter.Thefinalcomponentoftheairbomevideosystemistheantenna.GroundBasedOnboardAircraft BeijingUniversityofChemicalTechnologyMaster'sThesisFigure3.5:AerialVideoSystemDiagram3.3.6RADIoCoNTRoL,TRANSCEIVERThetransceiverisusedformanualcontrol;itisusedtochangecontrolfromautopilotmodetomanualcontrolmode.ThetransceiVerusedhereiStheFutaba6EX6.channel2.4GHzTransmitter/Receiver.TheaircraftisflownusingthistransmitterwhentheautopilotisoffandduringPilot-in-the—loopmodes.111etransmitterisnotneededforautonomousmodesandCanbeunpluggedwhentheautonomousmodesareenabled.3.3.7BYPASSCIRCUITTheBypasscircuitisanessentialcomponentoftheonboardavionicsduringautopilotdevelopment.Shouldaproblemariseduringthetestingthatputstheaircraftindanger,controlCanbeswitchedtotheRCcontr01.TlliSapproachtoautopilotdevelopmentallowedtestingofnewalgorithmsandhardwarewithminimalrisktotheaircraftorpeopleontheground.TlliSsectionwindiscussthedesignoftheBypasscircuit.TheArrinyprocessordecodesthepulsepositionmodulationservopositionsfromtheRCreceiVer.Channelsl_4and6.8aredecodedintovaluesrepresentingtheservopulsehightime.Channel5isdecodedseparatelyandisusedasthecontrolsignal.IfChannel5pulsefallsbelowasetthreshold,themicrocontrollerswitchesthedigitalmultiplexertoconnecttheRCreceiVerservooutputsdirectlytotheservos.TllismodeisknownasPilot—in.Controlmode(PIC).astheaircraftiscontrolledbyhumanpilotthroughthe72MHZcontrollink.Ifthechannel5pulserisesabovethethreshold,thedigitalmultiplexerconnectstheautopilotservooutputstotheservos.TllisisknownasComputer-in—Controlmode(CIC)astheautopilothascontroloftheaircraR.ThedecodedservopositionsandPIC/CICmodeinformationaresenttotheautopilotoveranasynchronousserialportat20Hz.Thedataissentregardlessofcontrolmode.DuringthePICmode,theautopilotlogstheservocontrolinformationwhichCanbeusedforflightanalysis.IntheCICmode.theautopilotCanbeconfiguredtousetheservopositionsignalsascontrolinputstothelOWPIDloopsforautopilot-assistedflight.TheautopilotCanalSObeconfiguredtousethecontrolsignalsinconjunctionwiththecontroleffortsofthePIDlcopstotestdisturbancerejection.3.3.8pOWERBUDGETTheavionicspowersourceisa8.4V1500mAhNiCadBatteryPack.Thebatteryisconnecteddirectlytothepowerdistributionboard.OnlytheSel'VOSandRCreceiverarepoweredbyanunregulatedvoltagesupply.ThisensuresthattheservoshaveaccesstoahighcurrentsourcethatmayberequiredintheeventofastuckSerVO.Ifa standardregulatorwereuseddunngthisevent,itwouldeithershutdownorfail,renderingtheaircraftuncontrollable.3.4.GROUNDSTATIoNThefinalsectionofthischapterdeals、历ththehardwareusedinthegroundstation.ThepurposeofthegroundstationhardwareistocontrolandmonitortheU黼andthepayload.ThegroundstationhardwareCanbedividedintotwosysterns.Thefirstistheautopilot.specifichardware.Thesecondisthepayloadspecifichardware.Therearethreecomponentsthat’Smakeuptheautopilot-specificgroundstationhardware.T11efirstisthedigitalmodem.Secondlysincethemodemshavea5kmrangewithouttheuseofdirectionalantennas,theAeroeommornni—directionalantennaisusedforthegroundstationcommunicationantenna.Thethirdcomponentofthegroundstationistheuserinterface.AlaptopcomputerrunningMicrosoftWindowsisused.Thepayload—specificportionofthegroundstationconsistsoftheground。basedhardwarenecessarytomakeuseofthepayloadcarriedbytheUAⅥIIlthecaseofthevideosurveillancepayload,avideoreceiveryantennaandvideodisplayareneeded.TheFelswebRX612receiverreceivesthevideofromtheairplane.”CoNCLUSIoNInthischaptertheautopilothardwarewasdealtwithindetail,theairframeWasfirstpresentedandthenthepowerbudgetorratherpowerconsiderationsWasalsodiscussed.Finallythegroundstationhardwarewaspresented. Chapter4Chapter4TheDesignoftheAutopilotSoftwareTheautopilotsoftwareiswrittenwithoptimization,functionality,development,andcompactnessinmind.Theautopilotsoftwareisdividedintothreesections.Thefirstisthepre-mainsection.Thepre.maincodeisresponsibleforinitializationoftheautopilothardware,declarationofglobalvariables,andinitializationof皿obalvariablesandstructures.Thenextsectionoftheautopilotcodeisthemainloop.Themainlooprunscontinuouslyatapproximately130Hz,andisresponsibleforgatheringandprocessingsensordataandcomputingthelOW.1evelcontrolalgorithms.Thecostates,processesthatruninparallelwitllthemainloop,comprisetheremainingsectionoftheautopilotcode.Theeostatefunctionsexecuteatspecifiedintervalsoronanevent-triggeredbasis.4.1THEARDUINoSoFr、7I,A础neArduinosoftwareisanopen-sourceelectronicsprototypingplatformbasedonflexibleeasy-to-usesoftware.TheArduinosoftwareconsistsofadevelopmentenvironment(IDE)andcorelibraries.TheIDEiswritteninJavaandbasedontheProcessingdevelopmentenvironment.ItsSOurOecodecanbefoundintheapp/directory.ThecorelibrariesarewritteninCandC++andcompiledusingavr-gccandAⅥ己Libc.Theopen-sourceArduinoenvironmentmakesiteasytowritecodesanduploadittotheAutopilotboardviaI/O.Itrunsonwindows,MacOSXandLinux.nleenvironmentiswritteninJavathatservesasacodeeditorandcompiler.Thearduinoenvironmenthasthefollowingfunctions:l’IVcrify/Compilecodes21Checkforerrorsinthecode.31Uploadthealreadycompliedcodes.4.2SOFIWARESIMULATIoNTlliSmethodofsimulationWasdevelopedbytheDIYDRONESCommunity.ThesimulationrunsinX.planesimulator’whileⅡleArdupilotwhichisconnectedviatheFTDIcabletothepcreceiYeSsimulatedGPSdataoverserialanditretumsbackproposedservopositionsbackOVerserialaspartoftelemetryinformation.Variablestllatarerecordedincludelong]tude,latitude,altitude,waypointsanddistance.Thefollowingaretherequiredsoftwareandtheirfunctions:l1ModifiedArdupilotcode2、lX.Planesimulator3’lGoogleEarth4、ArduSimulator33 —————————————』堕也堡型旦!翌坚!堡21里!!竺!竺!坠!!旦21竺型坚箜c旦:!坠竺!!FUNCTIoNS1)ConnectstoArduPiloteverserialforsendingandreceivingdata.2)ConnectstoX-Planeonlocalhost3)ReadsdatafromX-plane(1at/lon/alt/course),sendingthesetoautopilotasGPSsentences4)SimulatingFMAcopilotstabilization011ailerons/elevators5)ReadsandDisplaystelemetryandservopositionsfromAutopilot6)SendsservopositionstoX-planetocontrolthrottleandrudder7)RecordsflypathandsendsittoGoogleEarthtodisplay.ME’I’HoDSProgrammingtheArdupilotboardfordesiredwaypointsinvolvesfirstobtainingtheGPSLongitudeandLatitudecoordinatesofthewaypoints,thenenteringtheGPScoordinatesofthewaypointsintothecodeinArduinoenvironmentanduploadingittotheboard.ThecoordinatesCanbeobtainedfromGoogleMapsunderthesatelliteCoDESAMPLESThefollowingcodeshowshowtoprogramthelatitudeandlongitudecoordinatesofagivenlocationintothecode.ThewaypointsshownbelowarethecoordinatesofInnsbruck,thewaypointcoversAfling,Hotting,KranebittenandVols.voidsetupwaypoints(void){/*Declaringwaypoints*/wp_lat[1]=34.982613;wp__lon[1]=一118.443357;wpalt[1]=50;//meterswp_lat[2]=34.025136;wp_lon[2]=-I18.445254;wpalt[2]=100;//meterswp__lat[3]=34.018287;wp_lon[3]=-118.456048;wp_alt[3]=100;//meterswp_lat[4]--34.009332;wpIon[4]=-118.467672;wp_alt[4】’50;//meterswpJat[5]=34.006476;wplon[5]=-118.465413; wp_alt[5]250;//meterswp】m【6】_34009927;wpIon[6]=-118458320;wpall[6]=20;//meterFigu他4.1:ScreenShotoftheSimulationinX·PineFigure4.2ScreenshotofthesimulationinGoogleEarth 董;耋苎’=。:⋯一燕誊烈了:=:⋯Figure4.3:ScreenshotoftheArdumoSogwamAdvantagesoftheArduiaoSoftware1)Cross-PIatform:TheArduinosoftwarez11115ontheWindows,Ma岛ntoshOSxandLinuxoperatingsysten-is2)Itisasimple.clearprogrammingenviroranent3)Itisanopensourceandextensiblesoftware4)11lelanguagecanbeexpandedthrou曲cHlibrariesandAVR-CcodeandcanbeaddeddirectlyIntotheArduinopro汀a/llS4.3MAINLOOP(PID)Themainloopexecutesmecorealgorithmsresponsibleforlow—levelcentrelandSellsorprocessingThemainloopisexecutedmolefrequerltlythananyotherportionoftheautopilotcodeandexecutesaltherateofapproximatelyl30HzmtOpofthemainloopexecutesthefunaionswhichgatherandprocessthesensordataTheattitudeestimationalgorithmsareexecutedaspartofthese/lsorprocessingfunctionsThesefunctionswillbediscussedlndetuilinthefollowingsectionsAtthebottomofthemainloop,thelow—levelPIDeffurlsandPilot—in—theLooDcommandsarewrittentotheselwoactuators4.4ATTITLIDEESTIMATIoNTheautopilotusestwoseparateattitudeestimationfilterstodeterminetheaircraftattitudeThefirstisusedtoestimatetherollandpitchanglesThetechniqueemployedinthisfilterusesintegratedvaluesfromtherategyrosandestimatedrollandpitchanglesproducedbyacceleromcters.Thepurposeofthistechniqueisto prodIuceestimatesofrollandpitchthathavelowdriftanddecentaccuracy.ThesecondfilterisusedtoproduceaheadingestimatethathasafasterresponsethanGPSheading.4.5AHRSANDINSDESIGNThissectionpresentsthedesignofextendedKalmanfilters(Erd:)forprovidingangularandlinearpositionsofthevehicleandestimatingthecorrespondingbiasesonbothrategyrosandaccelerometers.AttitudeHeadingReferenceSystem—。ThebaSic胁e、ⅣorkoftheEKFforAHRSinvolvesestimationofthestateofcontinuoustime,nonlineardynamicsystem,矗刁‰,d+叫Yl=hik)+H(4.1)WhereXl=[qb∞】r∈妒。1representsthestateofthesystemthatiscomposedofthequaterion,q=k虿r∈吼4m,andrategyrobiases,吃∈锨3xl,(DE吼3一istheknowna119ularrates,and,Yl=(a驴'r.s)efft4“comprisestheaccelerationmeasurement,inmev“desbodyaxesandthemagneticheadingangle.TheprocessnoiseisgivenbywlandthemeasurementnoiseisgivenbyV卜Nonlinearfunctions,fl(Xl,co)andhI(x1)aregivenby:^G。.彩)=h。(x,)=‘。。q2-q3q0gl—93q2一glq003Jlp一9一,.一2gb2q3一qoq1)gG孑一qf—q;+q纠.咖叫(者q垮qI豢q生q3]L舌+‘一i一/37(4.2)(4.3)pg,幻%肌1%如1P..........。。.。.。...........。.L1—2 BeijingUniversitvofChemicalTeclIrIologyMaster’sThesis4.6lNERTIALNAⅥGATIoNSYSTEMThenonlineardynamicsystemoftheEKFfortheINSisgivenby毫=五k,口7)+%Y2=h2(x2)+V2(4.4)Where屯=[pvb,y∈倪9x1representsthestateofthesystemthatiscomposedoftheposition,P=阢①五riIltheLLAcoordinate,velocity,矿=h%%rinthelocalNEDframe,andaccelerometerbiases,ba∈赞3xl,at=k%口D】ristheaccelerationsintheNEDcoordinate,and儿=Gv,)∈吼缸1istheobservedmeasurementnoiseisgivenbyv2.Nonlinearfunctions,f2(x2,a)isgivenby五k,口7)=兄+hvE沁+h)cosA—v2。sin2一VE(如+h)cosA堡监璺垫墨沁+h)cos3,+鲁兰+aN-hahaNR旯++音兰+%一6haE.肌+。一南一南h+g+%^沁+厅)沁+)喵~D‰6∈孵3订砌hz(x2)=16娟106x3-jTf2.38(4.5)(4.6) Chapter5CONCLUSIONANDRECOMMEDATIONSOveraperiodoflessthantwoweeks,threedaysofextremelysuccessfulflighttestswereconducted.Inthistime,thelongitudinalandlateralcontrollersandlateralcontrollersaswell嬲thenavigationalgorithmwereshowntoworksuccessfullywimminimalflightcomplications.Thelackofcomplicationscanbelargelyattributedtothevastamountofsimulationthattookplacebeforetheflighttests.5.1FLIGHTTESToVERⅥEWAchecklistWillscreatedtoimprovethechancesofsuccessforeachflighttest.Thechecklistreligiouslyconsultedbeforeeachflighttestday,toensurethatnothingwasoverlookedandthattheaircraftWaSstillfullyintact.ThechecklistWaSalsoconsultedjustbeforeindividualflightteststoonceagainensuretheintegrityoftheaircraft.FLIGHTTEST1April12m2009wasthefirsttimethattheaircrafteverflew.Fiveflighttestswereconductedontheday,thefirstthreeofwhichinvolvedtrimmingtheactuatorsforsteadyflightandallowingthesafetypilottofamiliarizehimselfwiththeaircraft.Thelasttwoflightsinvolvedthetestingoftheyawratecontroller.Afterafewminorcomplications.theyawratecontrollerWaSfoundtoworkexcellently,winlthesafetypilotcontrollingonlytheelevatorandthethrottle.FLIGHTTEST2TheaimofthisdayWaStotesttheairspeedandclimbratecontrollerwiththesafetypilotcontrollingtheaileronsandtherudder.TwoflighttestswereconductedandtheairspeedandclimbratecontrollerWaSfoundtoworkimmediately.Duringthesecondflight,thealtitudeandheadingcontrollerswerearmedandalSOworkedimmediately.砀eSOcontrollerswereexpectedtoworkwithoutmucheffortbecauseofthelayeredcontrollerarchitecture.TheexcitementofallofthecontrollerSUCCESSledtothesecondtestflightcontinuingforOVerl5minutes.FLIGHTTEST3111epurposeofthisfinaltestingWaStotesttheNavigator.Fiveflighttestswereheldandaftersomeminorcomplications,thenavigatorWaSarmedandfoundtoworkexcellently.Itsuccessfullygeneratedandfollowedtheshortestpathmadeupofaninitialcircle,astraightlineandafinalcirclebetweentworespectivewaypointsin也ewaypointlist.ItWaSalSOpossibletoeditwaypointsandtoforcetheaircrafttoskipwaypoints.Manymoremonthscouldhavebeenabsorbedinthefinetuningandtestingallthedetailsoftheautopilot,butwitllthegoalofautonomousnavigationina39 璺型!竖望型∑!翌!望21曼垒宝型!苎!!壁垒翌竺12型坚竺坚:!!兰竺塑.minimumnumberoftestflightsachieved.5.2TELEMETRY/AUToPILoTRESUI』TSInthissectiontelemetrydatarepresentativeoftheperformanceoftheautopilotisprovided.Itmustbenotedhowever,thatthedatapresentediswhatthesensorsweremeasuringduringtheflightandnotwhattheactualstateoftheaircraftWas.Thesensordataprovidesagoodindicationofthecontrollerperformancebutpaintsanoisierpicturethanthetrueresponse.5.2.1LoNGITUDINALCoNTRoLLERSTypicallongitudinalflightdataaccumulatedoverailentireflightisshowninFigure6.1.111eaircraftisundermanualcontrolforthefirst195s.afterwhichtheautopilotisarmedwiththeairspeedandaltitudecontrollersactive.TheairspeedcommandWassetat25m/swhilethealtitudecommandwassetat100m.Withtheautopilotarmed.theRMSerrorinairspeediscalculatedas0.86m/swhiletheI蝴Serroriscalculatedas1.38m.Theseerrorswerebasedonthesensormeasurementdataandthusincludetheerrorsduetosensornoise.TheGPSreceivet’sgroundvdoCiWandaltitudemeasurements,usedtocalibratetheairspeedandbarometricaltitude,havealsoplottedontheairspeedandaltitudeplots.Theairspeedandgroundspeedareverysimilarwiththedifferencesarisingfromwindgusts.sensornoiseandtheaircraft’Sorientation.Howevertheresultsdoindicatethattheaircraftcouldbecontrolledwithouttheneedfortheairdatasensors. l’帅融“^m’edI⋯...—飞^.一,‘\——广V二卜一j+’1|r一:一:一一了一≮‘:Z⋯一:l—I=’l-esFAffeArduael——GPSAltitudeO∞1∞删2503∞∞O4∞4∞∞05∞伯他IslFigure5.1:Airspeed,Climbrate,andAltitudeforallentireflightAirSpeedControllerA5m/sstepresponseinairspeed,togetherwiththetheoreticalresponsefromtheblockdiagramsimulatorisshowninFigure6.2.Theactualairspeedresponsedisplaysmoreovershootthatthesimulatedresponse.Toimprovethisresponse.moreeffortmustbeinvestedintothemodeling.However,evenwiththeslightlydegradedresponse,thecontrollerwasfoundtoregulatetheairspeedsatisfactorily.一差一誊叠ulv写至嚣芷譬8姗懈。伽蔓宴霉 n睫。圈Figure5.2:AirspeedControllerstepresponse.CLlMBRATECoNTRoLLERTheclimbrateandaltituderesponsestoa2m/sstepcommandinclimbrate,铀.eshowninFigure6.3.Thetheoreticalresponsesfromtheblockdiagramsimulatorarealsoshownplottedonthesameaxes.42警董●蛩曩譬登莲 T翦譬。嗍Figure5.3:ClimbrateControllerstepCommandTheinheritnoiseoftheclimbratesensormakesitdifficulttoidentifytheaircraft’Strueresponsetotheclimbratestepconunand.However,astrongsimilaritybetweentheactualandsimulatedstepresponsecanbeseen.A1月ITUDECoNTRoLLERTheelimbrateandaltituderesponsestoalOmstepcommandinaltitudeareshowninFigure6.4,togetherwiththetheoreticalresponsesfromtheblockdiagramsimulator.Thestepcommandisissuedafter5sanditisclearthattheactualaltituderesponseisverysimilartothetheoreticalresponse.Theclimbrateandaltituderesponsetoa40mstepcommandareshowninFigure6.5.Thetheoreticalresultsareplottedonthesameaxes.Notehowthecontrolsysternautomaticallyenterstheaircraftintoaconstantclimbratemodeforthemajorityofthealtitudestep.Thealtituderesponseclearlyverifiestheworkingofthealtitudecontroller.43 呈墅!竖型翌!兰型堕垡£垒宝竺!!呈!!竺!璺竺12型坚箜!竺!!垒竺!!伽一圈Figure5.4:AltitudecontrollerstepresponsewithoutconstantclimbratephaseFigure5.5:AltitudeControllerstep麟ponse谢thconstantclimbratephase.嚣量。罨茁辱鬈u一量。雹霉≮ 5.3LATERALCoNTRoLLERSYAWRATECoNTRoLLERAyawratestepresponseisshowninFigure6.6.Thefactthatthereisadverseyawillustratesthesensitivityofopenloopcontroltotheaircraftdynamicmodel.Tocountertheeffectofadverseyaw,thewashoutfilterequationmustbeadjusteduntilsatisfactorypracticalresponseisattained.Another,morerobustapproach,wouldbet0providefeedbackoftheyawratesignaltotherudderattheonsetoftheturn.TheairspeedandaltitudecontrollerswerealSOarmedforthedurationoftheyawratestepcommands.TheirresponsesarealsoploRedtogetherwiththetheoreticalresponses.Figure5.6:LongitudinalandLateralcontrollercoupling.45 15蔓’o意5篮差。卜一⋯:、√、。,,LJl,:’‘’l,。I、‘Ⅵ_厂Vl=二鲕Frmgh山tT甜est。D砌atalO510'5202530354045I——FI码嫩Testoa媳I一I,’l一一&mula-orD出I∥—l∥I●/,,。::·lZ毋。。I。一≯●1髟7;I...‘.....彩I1∥’I/。1∥I...L.....形z··-I·-1Il/彳,,C’Z。翻幢嘞Figure5.7:Yawratecontroller.HEADINGCoNTRoLLER'5105O,舟^l二=裂瑟器警l~.,’于⋯‘’弋r‘,,fk、I_‘遗b—A,..j⋯V,/■弋●●、,、/一_,一V⋯\i一丁一;VlI2●6B,O1214t6'8一⋯-厂,一,/。一’?厂’?7Z‘少’;‘r|oH”Ⅲ=KtV_‘-a。一一S的u‰orDat,a‘StepComrnancl.O24681a121●1018Time一闺一墨零茎零曼。誊芷蠢1人∞矩∞勰∞o位—霎。暑iz Figure5.8:Headcontrollerstepresponse.Theyawrateandheadingresponse,toa45degreestepcommandinheading,areshownin6.8.Thesimulationresponsesareshownplottedonthesameaxes·Thestepcommandisissuedafter5sanditisclearthatthepracticalheadingresponsecloselymatchesthetheoreticalresponse.11腱。fsl47 曼业i翌曼竺里!翌!!堡21堡垒!里堕璺!堡垒望竺12墅坚竺!!羔坠竺堡Figure5.9:Headingcontrollerstepresponse111eyawrateandheadingresponsetoa135degreestepcommandinheadingareshowninFigure6.9.withthetheoreticalresultsplottedonthesameaxes.Notehowthecontrolsystemautomaticallyenterstheaircraftintoaconstantyawratemodeforthemajorityoftheheadingstep.neresultsclearlyverifytheworkingoftheheadingcontroller.SUMMLARYTKSchapterdescribedthethreedaVSofflighttestsinwhicha11ofthecontrollersweretestedandshowntoworkinpractice.Longitudinalandlateralflightdatawerepresented.Ingeneral.thepracticalresultswerefoundtomatchthetheoreticalresultsextremelywell.Explanationsforanydeviationsofthepracticalresultsfromtheoreticalresultswereprovided.TKSthesishasreportedthedesignandimplementationofanautopilotforaconventionalmodelaircraft.neaircraft’SdynamicmodelWasdevelopedasafunctionofitsphysicalparametersalone,whichallowsittoberapidlyappliedtootherconventionalmodelaircraft.ThiSmodelwasthenanalvzedandcriticallyevaluatedbeforebeingusedintheautopilotcontrollerdesign.ControllerarchitecturesuitableforimplementationusinglOWcost,off-the-shelfSensorsWasdeveloped.Finally,allavionicspackageandgroundstationWasdevelopedandtheautopilot’Sexcellentflighttestresultswerepresented.ThepracticalsuccessoftheautopilotCanbelargelYattributedtothecontrollerarchitectureandtheextensivevalidationbynon-linearsimulations.5.4RECoM口ⅥENDATIoNSRecommendationsofhowthecurrentautopilotcanbeimprovedandextendedarediscussedbelow.Itisimportanthowever,torememberthatanyaddedcomplexitytoasystemaiSOtendstoincreaseitschancesoffailure.Thus,theadditionmustalwaysbeconsideredinlightofwhatthefinalautopilotgoalSare.1)Theautopilotdevelopeduptothispointhasundergonethreedaysofflighttestsinwhichitperformedautonomousnavigationwithnoanomalies.Extensivetestingandfurtherstudyisstillrequiredtoidentifyhowtheautopilotrespondstounwantedsituationssuchasstalls,spins,actuatorsaturationandotherpotentialscenarios.Thus,comprehensivepracticaltestingisrecommendedtoconsolidatetheautopilotdesign.21Thecurrentautopilothasthehardwarecapabilitytoperformallautonomoustake-off.TheGPSheadingmeasurementscanbeusedinacomplementaryfashiontocontroltheaircraft’Srudderandnosewheel,tosteertheaircraftdowntherunway.TheairspeedsensorCanbeusedtodeterminewhentoissuetheelevatorcommandforliftO俄Onceintheair.theairspeedandclimbratecontrollerdesignedinthisprojectCanbeusedtoguidetheaircraftawayfromtheground.Thegraphicalsimulatorwillproveveryusefulduringthedevelopmentphaseofthecontroller.3)T0extendtheautopilottoperformanautonomouslandingwouldatleast Chapter5requiretheadditionofhi曲precisionaltitudesensorforthefinalstagesoflanding.ProvisioniSmadeinthecurrenthardwareandsoftwaredesign.fortheadditionofaPolaroidultrasonicrangefinder.ThiSsensoriscapableofprovidingsub—centimeteraccuracyaltitudemeasurementsoverarangeoflOm.andwillthuscomplimentthecurrentbarometricaltitudemeasurementwell.forthepurposesoflanding.Strictlateralcontrolfor1andingwillalSOrequireadditionalsensorssuchasvisionbasedorl心sensors.However,iflaterallocationisnotcritical(widerunway).thenthecontrollersdesignedinthisprojectwillsuffice.Againthegraphicalsimulatorwillproveveryusefulinthedesignofthiscontroller.4)Gainschedulingiscommonlyusedtoenhancetheperformanceofautopilots.Duringconventionalflight,theperformancebenefitsareoftennegligibleandtheaddedeffortofimplementationmakestheapproachunattractive.However,iftheautopilotisextendedtoperformautomatictakeoffsandlandings.andthengainschedulingisrecommended.ThiSwillensurethatthefeedbackremainValidfortheaircraft’Sairspeedandorientationduringeithermaneuver.5)Theautopilottelemetrydataindicatedthattheaccuracyofthegroundspeedandaltitudemeasurements,providedbythecurrentGPSreceiver,issimilartothatofthedifferentialandabsolutepressuresensors(whenthereislittlewind).TheGPSreceiver’SmeasurementaccuracysuggeststhattheautopilotcouldbeimplementedwithouttheneedfortheairdataSenSOrs.ItisrecommendedthatthisapproachisconsideredbecauseitminimizedtheSenSOrsrequiredfortheautopilotandremovestheneedforapitot-statietubestructure.However,theGPSonlyprovidesameasurementofgroundspeedandthuscaremustbetakentoensuretheautopilotdoesnotstalltheaircraftinwindyconditions.49 ARDUPILoTCoDESAppendixA//Definingrangesoftheservos(andESC),mustbe+一90de国陀es.#definemax16一throttle2100//ESCmaxposition,giveninuseconds,inmyunglySBTVOS2100isfine,youcantry2000..#defineminl6——throttle1000//ESCposition#definemax16_yaw2100//Servomaxposition#defineminl6yaw1000//Servorainposition#definereversejaw1//normal=0andreverse=1;UsethisifyourTransmitterissetup、航mtherudderchannelreversedl帆Dmaxandmins#defineheading_max15#defineheading_min一15#definealtitudemax40#definealtitudemin.45//Numberofwaypointsdefined#definewaypoints6#definedistancelimit4姒论|nhemaxdistanceallowedtotravelfromhome。/奉宰奉木宰宰幸木枣木毒奉枣木书书幸幸木母幸奉木●}}木●毒幸j|}●幸幸木幸枣木●幸幸幸宰木掌掌掌●事幸奉幸●奉●宰宰宰宰●串宰奉●枣+宰幸奉宰奎乖宰RTL,ifit担set弱truebytheuser,theautopilotwillalways宰justreturntothelaunchlat/lon(wayJat[0],way_Ion[O])whenenabled,宰maintaininginitialaltitude宰宰宰幸木}幸枣木木枣木枣木木木拳枣木木奉枣木幸宰木木拳木毒}木}木幸幸奉●木幸幸幸幸幸宰幸木}}幸●宰宰事●+●木奉奉木木幸枣●奉●●木宰书宰宰幸/#defineRTL1//0=waypointmode,1=Returnhomemode//PIDgains//Atthebeginingtrytouseonlyproportional..//Theoriginalconfigurationworksfineinmysimulator..#defineKp_heading10#defineKi_heading.015l #defineKd_heading0.001Kp_altitude4Kialtitude0.001l殂altitude2/宰宰宰木木木奉幸木枣木幸幸●宰宰宰●幸}幸奉木宰誊宰幸牛●奉謇///Definingwaypointsvariablesfloatwp_lat[waypoints+1];floatwp_lon[waypoints+1];intwp_alt[waypoints+1];bytecurrentWP=I;//Thisvariablesstorestheactualwaypointwearetryingtoreach..bytejurnplock_wp=O;//Whenswitchingwaypointsthislockwillallowonlyonetransition..bytewp_home_lock=-0;//intwp__bearing=0;//StoresthebearingfromthecurrentwaypointunsignedintwP.distance=0;//Storesthedistancesfromthecurrentwaypoint/木枣木幸幸书睾宰宰串掌掌拳木宰唪毒枣木奎毒幸幸幸聿卑···宰卑///PIDloopvariablesunsignedintheading_PID.timer;//TimertocalculatethedtofthePIDintheading_previous_error;floatheading_l;//StorestheresultoftheintegratorfloatheadingD://Storestheresultofthederivatorintheading._output;//StorestheresultofthePIDloopunsignedintaltitude_PID_timer;//TimertocalculatethedtofthePIDintaltitude_previous_error;floataltitudel;//Storestheresultoftheintegratorfloataltitude_D;//Storestheresultofthederivatorintaltitude_output;//StorestheresultofthePIDloop//PIDKconstants,definedatthebeginingofthecodefloatkp[]={Kp_heading,Kp_altitude};floatki[]={Kiheading,Ki_altitude};floatkd[]={Kd_heading,Kdaltitude};/奉幸奉拿率簟宰宰事枣木幸幸枣枣母宰·奉奉丰宰宰串掌幸串宰幸乖搴/charbuffer[90];//SerialbuffertocatchGPSdatacharheadnncfl="GPRMC”;∥GPSNMEAheadertolookforcharheadgga口=”GPGGAH;//GPSNMEAheadertolookforbyteunlock=l://somekindofeventflagbytechecksum=0;//thechecksumgenerated52 bytechecksumreceived=0;//Checksumreceivedcounter=0;//generalcounter.|鲁(疆SPointers*/char·token;char掌search=”,¨;char事brkb,幸pEnd;//Temporaryvariablesforsometasks,speciallyusedintheGPSparsingpart(LookattheNME氏jarsertab)unsignedlongtemp=0;unsignedlongtemp2=O;unsignedlongtemp3=O;.||Qsobtainedinformationfot_position=0;//Validgpspositionfloatlat--O;//CurrentLatitudefloatlon=0;//CurrentLongiitudeground_speed=0;//Groundspeed?yesGroundSpeed.intcourse=O;//Courseoverground...intair--O;//Altitude,.1l执心^Evariablesgps_new_data_flag=0;HAsimpleflagtoknowwhenwe’vegotnewgpsdata.unsignedintlaunch—altitude=O;//launchaltitude,altitudeinwaypointsisrelativetostartingaltitude.intmiddlethr=90;|Rhecentralpositionintmiddle_yaw=90;|№enetralpositionofyawmiddle_measurement_lock=0;//Anotherlocktovoidresettingthemiddlemeasurement../幸木事·串宰牛·士}木幸奉宰木宰宰宰车木幸木母枣木幸掌幸奉}宰//Nariablesusedintesting,willgoawaysomedayinttest=O;longtest2=O:inttest3=0;voidsetup(){Serial.begin(4800);S谢a1.prinfln("ArduPilotl!!”);//DeclaringpinspinMode(2,INPUT);//Servoinput;pinMode(3,INPUT);//ServoInput;53 pinMode(4,INPUT);//MUXpin∥ThisnextlineisaModepin,whichonlyworksinwiththree—positiontoggleswitchesonyourtransmitter.t|Ifyouwanttouseitforsomespecialmode,youmustchangetheAttinycode.Itisnotcurrentlyactive.//Whenyouputtheswitchinthecentralpositiontheattinywillsetllighapincalled”mode”,andyouCalluseittodowhateveryouwant⋯pinMode(5,INPUT);//Modepin(seeabove)pinMode(11,OUTPUT);//SimulatorOutputpinpinMode(12,OUTPUT);//LOCKLEDpininardupilotboard,indicatesvalidGPSdatapinMode(13,OUTPUT);//STATSLEDpininardupilotboard,blinkstoindicatetheboardisworkingwell⋯InitservoO;//InitalizingSeI'VO,see”Servo—Control”tab.//Testingservos(maxandmins),weareawareofthepropellerandwon'tspinthatwithoutwarning.一_)//test_throttle();testyaw();setup_waypoints0;//Seetab”Mission_Setup”//yeahado—whileloop,checksOVerandOVeragainuntilwehavevalidGPSpositionandlatisdiferentfromZero.HIre-verifytheLatbecausesometimesfailsandsetshomelat嬲zero.Thiswaynevergoeswron吕.do{gps_parse_nmeaO;//ReadingandparsingGPSdata)while(((fix_position!=Ox01)lt(1a净----O)));//Anotherverificationgpsnewdataflag=O;do{gps_parse_nmeaO;//ReadingandparsingGPSdata)while((gps_new__data_flag&0x011=0x01)&(gps_new_data_flag&Ox02l=Ox02));yaw_control0;HI。veputthisherebecauseineedtocalculatethedistancetothenextwaypoint,otherwiseitwillstartatwaypoint2. launch_altitude=alt;//Savingthelaunchaltitude,altitudeinwaypointsisrelativetostartingaltitude.//Storingthehomepositionwp_lat[0]=lat;//Savinghomelatitudewp_lon[0]=lon;//Savinghomelongitudewp_alt[o]=loo;//Storinghomealtitudeplus100,otherwiseitwilltrytomaintainthelaunchaltitude,youreallydon'twantflytoolow..//Printingthevaluesjus,tobesBre⋯Serial.println((10ng)(wp_lat[0]宰1000000));Serial.println((10ng)(wp_lon[O]}1000000));Serial.println((wp_alt[O]));voidloopO{//TurnontheLEDsifwehavevalidposition.(¨L0ck"LEDonardupil00,Yesiknowhorriblewhite.We’11switchtoblueinthenextversionofthehardwarei坟fixj)osition==Ox01){digitalWrite(12,HIGH);)else{digitalWrite(12,LOW);)digitalWrite(11,digitalRead(4));//Justforwaringpin4topin11,forsimulationporpuses⋯/●奉搴事十奉木宰}宰+木木木幸木宰木枣木木}幸+幸木木}宰}●幸奉宰幸宰}十奉宰●+}|I●宰木奉+事幸+宰牛宰宰木宰宰木宰枣宰卑宰,I宰宰幸幸毒●幸奎GPSfunction,readsandupdatealltheGPSdata...Thisfunctionislocatedin”GPS_Navigation”tab幸木木木幸幸宰幸毒枣枣木枣木枣木幸木枣木拳木书枣枣木木}奉●奉拳枣木枣枣木书木宰枣木木奉枣枣母木幸掌事幸宰木}++宰木+木奉宰宰幸宰宰●●木宰木枣/55 gps_parsenmeaO;/宰+木幸幸宰枣木木拳幸枣枣木木木木掌枣奉奉幸幸枣木枣木宰掌宰宰宰枣木幸●木●●●木宰掌●}奉}牛枣木木奉}宰书}事●●}宰}奉宰宰木木●宰木宰木●宰幸幸木宰木}奉●●●幸木幸●木木幸掌木木木●幸奉幸●●,I木事木宰幸●掌掌幸}宰jlc宰宰宰宰幸幸奉枣●幸宰事宰牛宰水掌誊木木宰宰●木●●●●木幸幸幸/if((gpsnew_dataflag&Ox01)=Ox01)//CheckingnewGPS”GPRMC¨dataflaginposition{digitalWrite(13,HIGH);gps_new_data_flag&=(--一Ox01);//Clearingnewdataflag⋯yaw_control();//Thisfunctionislocatedin"AutoPilot"tab.if(get_gps_dist(1at,Ion,wpJat[0],fromhomeisgreaterthan2000meters{current_wp=O;//Returnhome...>wp_lon[0])>distance_limit)//Ifthedistance/*Primvalues,justfordebugging事///Warningifyouareusingabaudrateof4800thiswillsuperslowdownthesystem..Serial.prim("Mot:”);Serial.print(tes3);Serial.print(”Yaw:”);Serial.print((int)test);.|≮Serial.print(”OrS"”);Serial.print(course);Serial.print(”W.PDir:");Serial.print((int)wp_bearing);Serial.print(”DAlt:”);Serial.print(wp_alt[currentwp]);Serial.print(”Dst:”);Serial.print(wp_distance); Serial.print(”Ⅵ伊:”);Serial.println((int)currentwp);幸/Serial.print(”AIt:¨);Serial.print((int)alt-launch_altitude);Serial.print(”Crs:”);Serial.print(course);Serial.print(”Dis:");Serial.print(wp_distance);Serial.print(”Des:”);Serial.println((int)wp_bearing);/誊掌木枣掌+幸事宰●卑●}木●●宰幸奉木奉宰木}●}宰木●●●木●奎奎●}毒幸幸幸幸木宰水牛事●奎●●●●奉宰幸●●宰十●事●事●幸事●●事幸牛宰//母宰幸宰宰●●宰木幸宰●宰宰●|I木幸奎}奉●事幸事宰书●掌幸幸木幸事宰幸幸奉木●宰幸木宰串宰宰掌幸幸●宰}●●}木幸宰母宰枣木奎木木毒木宰●}●●/if((gps_new_dataflag&0x02)-一Ox02)///CheckingnewGPSnGPGGA”dataflag{gpsnew_data_flag&=(~ox02);//Clearingflagthrottle_control0;//Thisfunctionislocatedin”AutoPilot”tab./宰木●●木●枣木奉}宰宰宰奎母木木幸事木拳幸幸书●事事●幸事奉宰●枣幸卡牛宰幸奉事}枣木,I宰事奉}宰●宰木幸幸宰奎●木幸木木宰奉木宰宰搴枣木·宰宰//*Thisisjustaprototype,ensBrethatthechecksRTLmode书/autopilotwilljumpONLYONEwaypoint,,宰●奉●幸奉幸幸木幸宰幸奎幸枣●幸毒奎宰奎宰幸毒●幸幸幸●掌奎幸●●枣幸幸幸木幸掌幸,I书掌奉枣木:Ic枣木宰}●+睾宰}幸母誊宰掌事謇謇率事枣木}掌},if(RTL-一-0)//VerifytheRTloption(0=Waypointmode,1=ReturnHomemode){if((wp_distance<30)&&(jumplock_wp=0x00))//Checkingifthewaypointdistanceislessthan30,andcheckifthelockisopen{currentwp++;//Switchthewaypointjumplock_wp=0x01;//Lockthewaypointswitcher.if(current_wp>waypoints)//Checkifwe'vepassedallthewaypoints,ifyeswillreturnhome。.57 if(digitalRead(4)一LOW)//CheckstheMUXpintoseeifweareinmanualmode(Low=manual)
此文档下载收益归作者所有