APM飞控程序部分导读

导读：#include<AP_Scheduler.h>//主循环调度程序，///-*-tab-width:4;Mode:C++;c-basic-offset:4;indent-tabs-mode:nil-*-#defineTHISFIRMWARE"ArduCopterV3.1-rc5"/*Thisprogramisfreesoftware:youcanredistri//-*-tab-width:4;Mode:C++;c-basic-offset:4;indent-tabs-mode:nil-*-#defineTHISFIRMWARE"ArduCopterV3.1-rc5"/*Thisprogramisfreesoftware:youcanredistributeitand/ormodifyitunderthetermsoftheGNUGeneralPublicLicenseaspublishedbytheFreeSoftwareFoundation,eitherversion3oftheLicense,or(atyouroption)anylaterversion.Thisprogramisdistributedinthehopethatitwillbeuseful,butWITHOUTANYWARRANTY;withouteventheimpliedwarrantyofMERCHANTABILITYorFITNESSFORAPARTICULARPURPOSE.SeetheGNUGeneralPublicLicenseformoredetails.YoushouldhavereceivedacopyoftheGNUGeneralPublicLicensealongwiththisprogram.Ifnot,see</licenses/>.*//**ArduCopterVersion3.0*Creator:JasonShort*LeadDeveloper:RandyMackay*BasedoncodeandideasfromtheArducopterteam:PatHickey,JoseJulio,JaniHirvinen,AndrewTridgell,JustinBeech,AdamRivera,Jean-LouisNaudin,RobertoNavoni*Thanksto:ChrisAnderson,MikeSmith,JordiMunoz,DougWeibel,JamesGoppert,BenjaminPelletier,RobertLefebvre,MarcoRobustini**SpecialThanksforContributors(inalphabeticalorderbyfirstname):**AdamMRivera:AutoCompassDeclination*AmilcarLucas:Cameramountlibrary*AndrewTridgell:Generaldevelopment,MavlinkSupport*AngelFernandez:Alphatesting*DougWeibel:Libraries*ChristofSchmid:Alphatesting*DaniSaez:VOctoSupport*GregoryFletcher:Cameramountorientationmath*Guntars:Armingsafetysuggestion*HappyKillmore:MavlinkGCS*HeinHollander:OctoSupport*IgorvanAirde:ControlLawoptimization*LeonardHall:FlightDynamics,Throttle,LoiterandNavigationControllers*JonathanChallinger:InertialNavigation*Jean-LouisNaudin:AutoLanding*MaxLevine:TriSupport,Graphics*JackDunkle:Alphatesting*JamesGoppert:MavlinkSupport*JaniHiriven:Testingfeedback*JohnArneBirkeland:PPMEncoder*JoseJulio:StabilizationControllaws*MarcoRobustini:Leadtester*MichaelOborne:MissionPlannerGCS*MikeSmith:Libraries,Codingsupport*Oliver:Piezosupport*OlivierAdler:PPMEncoder*RobertLefebvre:HeliSupport&LEDs*SandroBenigno:Camerasupport**AndmuchmoresoPLEASEPMmeonDIYDRONEStoaddyourcontributiontotheList**Requiresmodified"mrelax"versionofArduino,whichcanbefoundhere:*/p/ardupilot-mega/downloads/list*//////////////////////////////////////////////////////////////////////////////////Headerincludes////////////////////////////////////////////////////////////////////////////////#include<math.h>#include<stdio.h>#include<stdarg.h>//Commondependencies#include<AP_Common.h>#include<AP_Progmem.h>#include<AP_Menu.h>#include<AP_Param.h>//AP_HAL#include<AP_HAL.h>#include<AP_HAL_AVR.h>#include<AP_HAL_AVR_SITL.h>#include<AP_HAL_PX4.h>#include<AP_HAL_FLYMAPLE.h>#include<AP_HAL_Linux.h>#include<AP_HAL_Empty.h>//Applicationdependencies#include<GCS_MAVLink.h>//MAVLinkGCSdefinitions#include<AP_GPS.h>//ArduPilotGPSlibrary#include<AP_GPS_Glitch.h>//全球定位系统干扰保护库#include<DataFlash.h>//ArduPilotMegaFlashMemoryLibrary#include<AP_ADC.h>//ArduPilotMegaAnalogtoDigitalConverterLibrary#include<AP_ADC_AnalogSource.h>#include<AP_Baro.h>#include<AP_Compass.h>//ArduPilotMegaMagnetometerLibrary#include<AP_Math.h>//ArduPilotMegaVector/MatrixmathLibrary#include<AP_Curve.h>//Curveusedtolinearlisethrottlepwmtothrust#include<AP_InertialSensor.h>//ArduPilotMegaInertialSensor(accel&gyro)Library#include<AP_AHRS.h>#include<APM_PI.h>//PIlibrary#include<AC_PID.h>//PIDlibrary#include<RC_Channel.h>//遥控通道库#include<AP_Motors.h>//APMotorslibrary#include<AP_RangeFinder.h>//Rangefinderlibrary#include<AP_OpticalFlow.h>//OpticalFlowlibrary#include<Filter.h>//Filterlibrary#include<AP_Buffer.h>//APMFIFOBuffer#include<AP_Relay.h>//APMrelay#include<AP_Camera.h>//Photoorvideocamera#include<AP_Mount.h>//Camera/Antennamount#include<AP_Airspeed.h>//neededforAHRSbuild#include<AP_Vehicle.h>//neededforAHRSbuild#include<AP_InertialNav.h>//ArduPilotMegainertial导航library#include<AC_WPNav.h>//ArduCopterwaypointnavigationlibrary#include<AP_Declination.h>//ArduPilotMegaDeclinationHelperLibrary#include<AC_Fence.h>//ArducopterFencelibrary#include<memcheck.h>//memorylimitchecker#include<SITL.h>//softwareintheloopsupport#include<AP_Scheduler.h>//主循环调度程序#include<AP_RCMapper.h>//RCinputmappinglibrary#include<AP_Notify.h>//Notifylibrary#include<AP_BattMonitor.h>//Batterymonitorlibrary#ifSPRAYER==ENABLED#include<AC_Sprayer.h>//cropsprayerlibrary//AP_HALtoArduinocompatibilitylayer#include"compat.h"//Configuration#include"defines.h"#include"config.h"#include"config_channels.h"//Localmodules#include"Parameters.h"#include"GCS.h"//////////////////////////////////////////////////////////////////////////////////cliSerial//////////////////////////////////////////////////////////////////////////////////cliSerialisn'tstrictlynecessary-itisanaliasforhal.console.Itmay//bedeprecatedinfavorofhal.consoleinlaterreleases.staticAP_HAL::BetterStream*cliSerial;//N.B.weneedtokeepastaticdeclarationwhichisn'tguardedbymacros//atthetoptocooperatewiththeprototypemangler.//////////////////////////////////////////////////////////////////////////////////AP_HALinstance////////////////////////////////////////////////////////////////////////////////constAP_HAL::HAL&hal=AP_HAL_BOARD_DRIVER;//////////////////////////////////////////////////////////////////////////////////Parameters////////////////////////////////////////////////////////////////////////////////////Globalparametersareallcontainedwithinthe'g'class.//staticParametersg;//mainloopschedulerstaticAP_Schedulerscheduler;//AP_NotifyinstancestaticAP_Notifynotify;//////////////////////////////////////////////////////////////////////////////////prototypes////////////////////////////////////////////////////////////////////////////////staticvoidupdate_events(void);staticvoidprint_flight_mode(AP_HAL::BetterStream*port,uint8_tmode);//////////////////////////////////////////////////////////////////////////////////Dataflash////////////////////////////////////////////////////////////////////////////////#ifCONFIG_HAL_BOARD==HAL_BOARD_APM2staticDataFlash_APM2DataFlash;#elifCONFIG_HAL_BOARD==HAL_BOARD_APM1staticDataFlash_APM1DataFlash;#elifCONFIG_HAL_BOARD==HAL_BOARD_AVR_SITL//staticDataFlash_FileDataFlash("/tmp/APMlogs");staticDataFlash_SITLDataFlash;#elifCONFIG_HAL_BOARD==HAL_BOARD_PX4staticDataFlash_FileDataFlash("/fs/microsd/APM/logs");#elifCONFIG_HAL_BOARD==HAL_BOARD_LINUXstaticDataFlash_FileDataFlash("logs");#elsestaticDataFlash_EmptyDataFlash;#endif//////////////////////////////////////////////////////////////////////////////////theratewerunthemainloopat////////////////////////////////////////////////////////////////////////////////staticconstAP_InertialSensor::Sample_rateins_sample_rate=AP_InertialSensor::RATE_100HZ;//////////////////////////////////////////////////////////////////////////////////Sensors////////////////////////////////////////////////////////////////////////////////////Therearethreebasicoptionsrelatedtoflightsensorselection.////-Normalflightmode.Realsensorsareused.//-HILAttitudemode.Mostsensorsaredisabled,astheHIL//protocolsuppliesattitudeinformationdirectly.//-HILSensorsmode.Syntheticsensorsareconfiguredthat//supplydatafromthesimulation.////AllGPSaccessshouldbethroughthispointer.staticGPS*g_gps;staticGPS_Glitchgps_glitch(g_gps);//flightmodesconveniencearraystaticAP_Int8*flight_modes=&g.flight_mode1;#ifHIL_MODE==HIL_MODE_DISABLED#ifCONFIG_ADC==ENABLEDstaticAP_ADC_ADS7844adc;#endif#ifCONFIG_IMU_TYPE==CONFIG_IMU_MPU6000staticAP_InertialSensor_MPU6000ins;#elifCONFIG_IMU_TYPE==CONFIG_IMU_OILPANstaticAP_InertialSensor_Oilpanins(&adc);#elifCONFIG_IMU_TYPE==CONFIG_IMU_SITLstaticAP_InertialSensor_HILins;#elifCONFIG_IMU_TYPE==CONFIG_IMU_PX4staticAP_InertialSensor_PX4ins;#elifCONFIG_IMU_TYPE==CONFIG_IMU_FLYMAPLEAP_InertialSensor_Flymapleins;#elifCONFIG_IMU_TYPE==CONFIG_IMU_L3G4200DAP_InertialSensor_L3G4200Dins;#endif#ifCONFIG_HAL_BOARD==HAL_BOARD_AVR_SITL//WhenbuildingforSITLweusetheHILbarometerandcompassdriversstaticAP_Baro_HILbarometer;staticAP_Compass_HILcompass;staticSITLsitl;#else//Otherwise,instantiatearealbarometerandcompassdriver#ifCONFIG_BARO==AP_BARO_BMP085staticAP_Baro_BMP085barometer;#elifCONFIG_BARO==AP_BARO_PX4staticAP_Baro_PX4barometer;#elifCONFIG_BARO==AP_BARO_MS5611#ifCONFIG_MS5611_SERIAL==AP_BARO_MS5611_SPIstaticAP_Baro_MS5611barometer(&AP_Baro_MS5611::spi);#elifCONFIG_MS5611_SERIAL==AP_BARO_MS5611_I2CstaticAP_Baro_MS5611barometer(&AP_Baro_MS5611::i2c);#else#errorUnrecognizedCONFIG_MS5611_SERIALsetting.#endif#endif#ifCONFIG_HAL_BOARD==HAL_BOARD_PX4staticAP_Compass_PX4compass;#elsestaticAP_Compass_HMC5843compass;#endif#endif//realGPSselection#ifGPS_PROTOCOL==GPS_PROTOCOL_AUTOAP_GPS_Autog_gps_driver(&g_gps);#elifGPS_PROTOCOL==GPS_PROTOCOL_NMEAAP_GPS_NMEAg_gps_driver;#elifGPS_PROTOCOL==GPS_PROTOCOL_SIRFAP_GPS_SIRFg_gps_driver;#elifGPS_PROTOCOL==GPS_PROTOCOL_UBLOXAP_GPS_UBLOXg_gps_driver;#elifGPS_PROTOCOL==GPS_PROTOCOL_MTKAP_GPS_MTKg_gps_driver;#elifGPS_PROTOCOL==GPS_PROTOCOL_MTK19AP_GPS_MTK19g_gps_driver;#elifGPS_PROTOCOL==GPS_PROTOCOL_NONEAP_GPS_Noneg_gps_driver;#else#errorUnrecognisedGPS_PROTOCOLsetting.#endif//GPSPROTOCOLstaticAP_AHRS_DCMahrs(&ins,g_gps);#elifHIL_MODE==HIL_MODE_SENSORS//sensoremulatorsstaticAP_ADC_HILadc;staticAP_Baro_HILbarometer;staticAP_Compass_HILcompass;staticAP_GPS_HILg_gps_driver;staticAP_InertialSensor_HILins;staticAP_AHRS_DCMahrs(&ins,g_gps);#ifCONFIG_HAL_BOARD==HAL_BOARD_AVR_SITL//WhenbuildingforSITLweusetheHILbarometerandcompassdriversstaticSITLsitl;#endif导读：1second)*8TBD*EachAuxchannelcanbeconfiguredtohaveanyoftheavailableauxiliaryfunctionsassignedtoit.*Seelibraries/RC_Channel/RC_Channel_aux.hformoreinformation*///DocumentationofGLoba1second)*8TBD*EachAuxchannelcanbeconfiguredtohaveanyoftheavailableauxiliaryfunctionsassignedtoit.*Seelibraries/RC_Channel/RC_Channel_aux.hformoreinformation*///DocumentationofGLobals:staticunion{struct{uint8_thome_is_set:1;//0uint8_tsimple_mode:2;//1,2//Thisisthestateofsimplemode:0=disabled;1=SIMPLE;2=SUPERSIMPLEuint8_tpre_arm_rc_check:1;//3//trueifrcinputpre-armcheckshavebeencompletedsuccessfullyuint8_tpre_arm_check:1;//4//trueifallpre-armchecks(rc,accelcalibration,gpslock)havebeenperformeduint8_tauto_armed:1;//5//stopsautomissionsfrombeginninguntilthrottleisraiseduint8_tlogging_started:1;//6//trueifdataflashlogginghasstarteduint8_tdo_flip:1;//7//Usedtoenableflipcodeuint8_ttakeoff_complete:1;//8uint8_tland_complete:1;//9//trueifwehavedetectedalandinguint8_tnew_radio_frame:1;//10//SettrueifwehavenewPWMdatatoactonfromtheRadiouint8_tCH7_flag:2;//11,12//ch7auxswitch:0isloworfalse,1iscenterortrue,2ishighuint8_tCH8_flag:2;//13,14//ch8auxswitch:0isloworfalse,1iscenterortrue,2ishighuint8_tusb_connected:1;//15//trueifAPMispoweredfromUSBconnectionuint8_tyaw_stopped:1;//16//UsedtomanagetheYawholdcapabilitiesuint8_tdisable_stab_rate_limit:1;//17//disableslimitsraterequestfromthestabilitycontrolleruint8_trc_receiver_present:1;//18//trueifwehaveanrcreceiverpresent(i.e.ifwe'veeverreceivedanupdate};uint32_tvalue;}ap;//////////////////////////////////////////////////////////////////////////////////Radio//////////////////////////////////////////////////////////////////////////////////Thisisthestateoftheflightcontrolsystem//TherearemultiplestatesdefinedsuchasSTABILIZE,ACRO,staticint8_tcontrol_mode=STABILIZE;//Usedtomaintainthestateofthepreviouscontrolswitchposition//Thisissetto-1whenweneedtore-readtheswitchstaticuint8_toldSwitchPosition;staticRCMapperrcmap;//receiverRSSIstaticuint8_treceiver_rssi;//////////////////////////////////////////////////////////////////////////////////Failsafe////////////////////////////////////////////////////////////////////////////////staticstruct{uint8_trc_override_active:1;//0//trueifrccontrolareoverwrittenbygroundstationuint8_tradio:1;//1//Astatusflagfortheradiofailsafeuint8_tbattery:1;//2//Astatusflagforthebatteryfailsafeuint8_tgps:1;//3//Astatusflagforthegpsfailsafeuint8_tgcs:1;//4//Astatusflagforthegroundstationfailsafeint8_tradio_counter;//numberofiterationswiththrottlebelowthrottle_fs_valueuint32_tlast_heartbeat_ms;//thetimewhenthelastHEARTBEATmessagearrivedfromaGCS-usedfortriggeringgcsfailsafe}failsafe;//////////////////////////////////////////////////////////////////////////////////MotorOutput////////////////////////////////////////////////////////////////////////////////#ifFRAME_CONFIG==QUAD_FRAME#defineMOTOR_CLASSAP_MotorsQuad#elifFRAME_CONFIG==TRI_FRAME#defineMOTOR_CLASSAP_MotorsTri#elifFRAME_CONFIG==HEXA_FRAME#defineMOTOR_CLASSAP_MotorsHexa#elifFRAME_CONFIG==Y6_FRAME#defineMOTOR_CLASSAP_MotorsY6#elifFRAME_CONFIG==OCTA_FRAME#defineMOTOR_CLASSAP_MotorsOcta#elifFRAME_CONFIG==OCTA_QUAD_FRAME#defineMOTOR_CLASSAP_MotorsOctaQuad#elifFRAME_CONFIG==HELI_FRAME#defineMOTOR_CLASSAP_MotorsHeli#else#errorUnrecognisedframetype#endif#ifFRAME_CONFIG==HELI_FRAME//helicopterconstructorrequiresmoreargumentsstaticMOTOR_CLASSmotors(&g.rc_1,&g.rc_2,&g.rc_3,&g.rc_4,&g.rc_8,&g.heli_servo_1,&g.heli_servo_2,&g.heli_servo_3,&g.heli_servo_4);#elifFRAME_CONFIG==TRI_FRAME//triconstructorrequiresadditionalrc_7argumenttoallowtailservoreversingstaticMOTOR_CLASSmotors(&g.rc_1,&g.rc_2,&g.rc_3,&g.rc_4,&g.rc_7);#elsestaticMOTOR_CLASSmotors(&g.rc_1,&g.rc_2,&g.rc_3,&g.rc_4);#endif//////////////////////////////////////////////////////////////////////////////////PIDs//////////////////////////////////////////////////////////////////////////////////ThisisaconvienienceaccessorfortheIMUrollrates.It'scurrentlytherawIMUrates//andnottheadjustedomegarates,butthenameisstuckstaticVector3fomega;//Thisisusedtoholdradiotuningvaluesforin-flightCH6tuningfloattuning_value;//usedtolimittheratethatthepidcontrolleroutputisloggedsothatitdoesn'tnegativelyaffectperformancestaticuint8_tpid_log_counter;//////////////////////////////////////////////////////////////////////////////////LEDoutput//////////////////////////////////////////////////////////////////////////////////BlinkingindicatesGPSstatusstaticuint8_tcopter_leds_GPS_blink;//Blinkingindicatesbatterystatusstaticuint8_tcopter_leds_motor_blink;//Navigationconfirmationblinksstaticint8_tcopter_leds_nav_blink;//////////////////////////////////////////////////////////////////////////////////GPSvariables//////////////////////////////////////////////////////////////////////////////////ThisisusedtoscaleGPSvaluesforEEPROMstorage//10^7timesDecimalGPSmeans1==1cm//Thisapproximationmakescalculationsintegerandit'seasytoreadstaticconstfloatt7=10000000.0;//Weuseatan2andothertrigtechniquestocalaculateangles//Weneedtoscalethelongitudeuptomakethesecalcswork//toaccountfordecreasingdistancebetweenlinesoflongitudeawayfromtheequatorstaticfloatscaleLongUp=1;//SometimesweneedtoremovethescalingfordistancecalcsstaticfloatscaleLongDown=1;//////////////////////////////////////////////////////////////////////////////////Location&Navigation//////////////////////////////////////////////////////////////////////////////////Thisistheanglefromthecoptertothenextwaypointincenti-degreesstaticint32_twp_bearing;//Theoriginalbearingtothenextwaypoint.usedtopointthenoseofthecopteratthenextwaypointstaticint32_toriginal_wp_bearing;//Thelocationofhomeinrelationtothecopterincenti-degreesstaticint32_thome_bearing;//distancebetweenplaneandhomeincmstaticint32_thome_distance;//distancebetweenplaneandnextwaypointincm.staticuint32_twp_distance;//navigationmode-optionsincludeNAV_NONE,NAV_LOITER,NAV_CIRCLE,NAV_WPstaticuint8_tnav_mode;//Registercontainingtheindexofthecurrentnavigationcommandinthemissionscriptstaticint16_tcommand_nav_index;//Registercontainingtheindexofthepreviousnavigationcommandinthemissionscript//Usedtomanagetheexecutionofconditionalcommandsstaticuint8_tprev_nav_index;//Registercontainingtheindexofthecurrentconditionalcommandinthemissionscriptstaticuint8_tcommand_cond_index;//UsedtotracktherequiredWPnavigationinformation//optionsinclude//NAV_ALTITUDE-havewereachedthedesiredaltitude?//NAV_LOCATION-havewereachedthedesiredlocation?//NAV_DELAY-havewewaitedatthewaypointthedesiredtime?staticfloatlon_error,lat_error;//Usedtoreporthowmanycmwearefromthenextwaypointorloitertargetpositionstaticint16_tcontrol_roll;staticint16_tcontrol_pitch;staticuint8_trtl_state;//recordsstateofrtl(initialclimb,returninghome,etc)staticuint8_tland_state;//recordsstateofland(flyingtolocation,descending)//////////////////////////////////////////////////////////////////////////////////Orientation//////////////////////////////////////////////////////////////////////////////////Convienienceaccessorsforcommonlyusedtrigfunctions.Thesevaluesaregenerated//bytheDCMthroughafewsimpleequations.Theyareusedthroughoutthecodewherecosandsin//wouldnormallybeused.//Thecosvaluesaredefaultedto1togetadecentinitialvalueforalevelstatestaticfloatcos_roll_x=1.0;staticfloatcos_pitch_x=1.0;staticfloatcos_yaw=1.0;staticfloatsin_yaw;staticfloatsin_roll;staticfloatsin_pitch;//////////////////////////////////////////////////////////////////////////////////SIMPLEMode//////////////////////////////////////////////////////////////////////////////////UsedtotracktheorientationofthecopterforSimplemode.Thisvalueisresetateacharming//orinSuperSimplemodewhenthecopterleavesa20mradiusfromhome.staticfloatsimple_cos_yaw=1.0;staticfloatsimple_sin_yaw;staticint32_tsuper_simple_last_bearing;staticfloatsuper_simple_cos_yaw=1.0;staticfloatsuper_simple_sin_yaw;//Storesinitialbearingwhenarmed-initialsimplebearingismodifiedinsupersimplemodesonotsuitablestaticint32_tinitial_armed_bearing;//////////////////////////////////////////////////////////////////////////////////Ratecontollertargets////////////////////////////////////////////////////////////////////////////////staticuint8_trate_targets_frame=EARTH_FRAME;//indicateswhetherratetargetsprovidedinearthorbodyframestaticint32_troll_rate_target_ef;staticint32_tpitch_rate_target_ef;staticint32_tyaw_rate_target_ef;staticint32_troll_rate_target_bf;//bodyframerollratetargetstaticint32_tpitch_rate_target_bf;//bodyframepitchratetargetstaticint32_tyaw_rate_target_bf;//bodyframeyawratetarget//////////////////////////////////////////////////////////////////////////////////Throttlevariables////////////////////////////////////////////////////////////////////////////////staticint16_tthrottle_accel_target_ef;//earthframethrottleaccelerationtargetstaticboolthrottle_accel_controller_active;//truewhenaccelbasedthrottlecontrollerisactive,falsewhenhigherlevelthrottlecontrollersareprovidingthrottleoutputdirectlystaticfloatthrottle_avg;//g.throttle_cruiseasafloatstaticint16_tdesired_climb_rate;//pilotdesiredclimbrate-forloggingpurposesonlystaticfloattarget_alt_for_reporting;//targetaltitudeincmforreporting(logsandgroundstation)//////////////////////////////////////////////////////////////////////////////////ACROMode//////////////////////////////////////////////////////////////////////////////////UsedtocontrolAxislockstaticint32_tacro_roll;//desiredrollanglewhilesportmodestaticint32_tacro_roll_rate;//desiredrollratewhileinacromodestaticint32_tacro_pitch;//desiredpitchanglewhilesportmodestaticint32_tacro_pitch_rate;//desiredpitchratewhileacromodestaticint32_tacro_yaw_rate;//desiredyawratewhileacromodestaticfloatacro_level_mix;//scalesbackroll,pitchandyawinverselyproportionaltoinputfrompilot//Filters#ifFRAME_CONFIG==HELI_FRAME//staticLowPassFilterFloatrate_roll_filter;//RateRollfilter//staticLowPassFilterFloatrate_pitch_filter;//RatePitchfilter#endif//HELI_FRAME//////////////////////////////////////////////////////////////////////////////////CircleMode/Loitercontrol////////////////////////////////////////////////////////////////////////////////Vector3fcircle_center;//circlepositionexpressedincmfromhomelocation.x=lat,y=lon//anglefromthecirclecentertothecopter'sdesiredlocation.Incrementedatcircle_rate/secondstaticfloatcircle_angle;//thetotalangle(inradians)travelledstaticfloatcircle_angle_total;//deg:howmanytimestocircleasspecifiedbymissioncommandstaticuint8_tcircle_desired_rotations;staticfloatcircle_angular_acceleration;//circlemode'sangularaccelerationstaticfloatcircle_angular_velocity;//circlemode'sangularvelocitystaticfloatcircle_angular_velocity_max;//circlemode'smaxangularvelocity//HowlongweshouldstayinLoiterModeformissionscripting(timeinseconds)staticuint16_tloiter_time_max;//Howlonghavewebeenloitering-Thestarttimeinmillisstaticuint32_tloiter_time;//////////////////////////////////////////////////////////////////////////////////CH7andCH8savewaypointcontrol//////////////////////////////////////////////////////////////////////////////////ThisregistertracksthecurrentMissionCommandindexwhenwriting//amissionusingCh7orCh8auxswitchesinflightstaticint8_taux_switch_wp_index;//////////////////////////////////////////////////////////////////////////////////BatterySensors////////////////////////////////////////////////////////////////////////////////staticAP_BattMonitorbattery;//////////////////////////////////////////////////////////////////////////////////Altitude//////////////////////////////////////////////////////////////////////////////////The(throttle)controllerdesiredaltitudeincmstaticfloatcontroller_desired_alt;//Thecmweareoffinaltitudefromnext_WP.alt–PositivevaluemeanswearebelowtheWPstaticint32_taltitude_error;//Thecm/swearemovingupordownbasedonfiltereddata-Positive=UPstaticint16_tclimb_rate;//ThealtitudeasreportedbySonarincm–Valuesare20to700generally.staticint16_tsonar_alt;staticuint8_tsonar_alt_health;//trueifwecantrustthealtitudefromthesonarstaticfloattarget_sonar_alt;//desiredaltitudeincmabovetheground//ThealtitudeasreportedbyBaroincm–Valuescanbequitehighstaticint32_tbaro_alt;staticint16_tsaved_toy_throttle;//////////////////////////////////////////////////////////////////////////////////flightmodes//////////////////////////////////////////////////////////////////////////////////FlightmodesarecombinationsofRoll/Pitch,YawandThrottlecontrolmodes//EachFlightmodeisauniquecombinationofthesemodes////ThecurrentdesiredcontrolschemeforYawstaticuint8_tyaw_mode;//Thecurrentdesiredcontrolschemeforrollandpitch/navigationstaticuint8_troll_pitch_mode;//Thecurrentdesiredcontrolschemeforaltitudeholdstaticuint8_tthrottle_mode;//////////////////////////////////////////////////////////////////////////////////flightspecific//////////////////////////////////////////////////////////////////////////////////Anadditionalthrottleaddedtokeepthecopteratthesamealtitudewhenbanking导读：//RepeatMissionScriptingComman//Thetypeofrepeatingevent-Toggleaservochannel,ToggletheAPM1relay,etcstaticuint8_tevent_id;//Usedtomanagethetimimngofrepeatingeventsstaticuint32_tevent_timer;//Howlongtodelaythenextfiringofeventinmillisstaticuint16_tevent_delay;//howmanytimestofire:0=forever,1=doonce,2=dotwicestaticint16_tevent_repeat;//percommandvalue,suchasPWMforservosstaticint16_tevent_value;//thestoredvalueusedtoundocommands-suchasoriginalPWMcommandstaticint16_tevent_undo_value;//////////////////////////////////////////////////////////////////////////////////DelayMissionScriptingCommand////////////////////////////////////////////////////////////////////////////////staticint32_tcondition_value;//usedinconditioncommands(egdelay,changealt,etc.)staticuint32_tcondition_start;//////////////////////////////////////////////////////////////////////////////////IMUvariables//////////////////////////////////////////////////////////////////////////////////Integrationtime(inseconds)forthegyros(DCMalgorithm)//UpdatedwiththefastloopstaticfloatG_Dt=0.02;//////////////////////////////////////////////////////////////////////////////////InertialNavigation////////////////////////////////////////////////////////////////////////////////staticAP_InertialNavinertial_nav(&ahrs,&barometer,g_gps,gps_glitch);//////////////////////////////////////////////////////////////////////////////////Waypointnavigationobject//To-Do:moveinertialnavuporothernavigationvariablesdownhere////////////////////////////////////////////////////////////////////////////////staticAC_WPNavwp_nav(&inertial_nav,&ahrs,&g.pi_loiter_lat,&g.pi_loiter_lon,&g.pid_loiter_rate_lat,&g.pid_loiter_rate_lon);//////////////////////////////////////////////////////////////////////////////////Performancemonitoring//////////////////////////////////////////////////////////////////////////////////ThenumberofGPSfixeswehavehadstaticuint8_tgps_fix_count;staticint16_tpmTest1;//SystemTimers//--------------//Timeinmicrosecondsofmaincontrolloopstaticuint32_tfast_loopTimer;//Counterofmainloopexecutions.Usedforperformancemonitoringandfailsafeprocessings