APM飛控程序解讀匯報

///-*-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: JasonShortLeadDeveloper:RandyMackayBasedoncodeandideasfromtheArducopterteam:PatHickey,JoseJulio,JaniHirvinen,AndrewTridgell,JustinBeech,AdamRivera,Jean-LouisNaudin,RobertoNavoni*Thanksto: ChrisAnderson, MikeSmith,JordiMunoz, DougWeibel,James Goppert,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,LoiterandNavigationControllersJonathanChallinger:InertialNavigationJean-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*AndmuchmoresoPLEASEPMmeonDIYDRONEStoaddyourcontributiontotheListRequiresmodified"mrelax"versionofArduino,whichcanbefoundhere:code.google./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>//MAVLinkGCS定義#include<AP_GPS.h>//ArduPilotGPSlibrary#include<AP_GPS_Glitch.h>//全球定位系統(tǒng)干擾保護庫#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導(dǎo)航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>//主循環(huán)調(diào)度程序#include<AP_RCMapper.h>//RCinputmappinglibrary#include<AP_Notify.h>//Notifylibrary#include<AP_BattMonitor.h>//Batterymonitorlibrary#ifSPRAYER==ENABLED#include<AC_Sprayer.h>//cropsprayerlibrary#endifAP_HALArduino兼容性層#include"compat.h"配置#include"defines.h"#include"config.h"#include"config_channels.h"Localmodules#include"Parameters.h"#include"GCS.h"http:////////////////////////////////////////////////////////////////////////////////cliSerial////////////////////////////////////////////////////////////////////////////////cliSerialisn'tstrictlynecessary-itisanaliasforhal.console.Itmaybedeprecatedinfavorofhal.consoleinlaterreleases.staticAP_HAL::BetterStream*cliSerial;N.B.weneedtokeepastaticdeclarationwhichisn'tguardedbymacrosatthetoptocooperatewiththeprototypemangler.////////////////////////////////////////////////////////////////////////////////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//////////////////////////////////////////////////////////////////////////////////運行主循環(huán)////////////////////////////////////////////////////////////////////////////////靜態(tài)常量AP_InertialSensor:Sample_rateins_sample_rate=AP_InertialSensor::RATE_100HZ;//////////////////////////////////////////////////////////////////////////////////Sensors//////////////////////////////////////////////////////////////////////////////////Therearethreebasicoptionsrelatedtoflightsensorselection.-Normalflightmode.Realsensorsareused.-HILAttitudemode.Mostsensorsaredisabled,astheHILprotocolsuppliesattitudeinformationdirectly.-HILSensorsmode.Syntheticsensorsareconfiguredthatsupplydatafromthesimulation.////AllGPSaccessshouldbethroughthispointer.staticGPS *g_gps;staticGPS_Glitch gps_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_SITLWhenbuildingforSITLweusetheHILbarometerandcompassdriversstaticAP_Baro_HILbarometer;staticAP_Compass_HILcompass;staticSITLsitl;#elseOtherwise,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#if GPS_PROTOCOL==GPS_PROTOCOL_AUTOAP_GPS_Auto g_gps_driver(&g_gps);#elifGPS_PROTOCOL==GPS_PROTOCOL_NMEAAP_GPS_NMEA g_gps_driver;#elifGPS_PROTOCOL==GPS_PROTOCOL_SIRFAP_GPS_SIRF g_gps_driver;#elifGPS_PROTOCOL==GPS_PROTOCOL_UBLOXAP_GPS_UBLOX g_gps_driver;#elifGPS_PROTOCOL==GPS_PROTOCOL_MTKAP_GPS_MTK g_gps_driver;#elifGPS_PROTOCOL==GPS_PROTOCOL_MTK19AP_GPS_MTK19 g_gps_driver;#elifGPS_PROTOCOL==GPS_PROTOCOL_NONEAP_GPS_None g_gps_driver;#else#errorUnrecognisedGPS_PROTOCOLsetting.#endif//GPSPROTOCOLstaticAP_AHRS_DCMahrs(&ins,g_gps);#elifHIL_MODE==HIL_MODE_SENSORS//sensoremulatorsstaticAP_ADC_HIL adc;staticAP_Baro_HIL barometer;staticAP_Compass_HIL compass;staticAP_GPS_HIL g_gps_driver;staticAP_InertialSensor_HIL ins;staticAP_AHRS_DCM ahrs(&ins,g_gps);#ifCONFIG_HAL_BOARD==HAL_BOARD_AVR_SITLWhenbuildingforSITLweusetheHILbarometerandcompassdriversstaticSITLsitl;#endif#elifHIL_MODE==HIL_MODE_ATTITUDEstaticAP_ADC_HIL adc;staticAP_InertialSensor_HIL ins;staticAP_AHRS_HIL ahrs(&ins,g_gps);staticAP_GPS_HIL g_gps_driver;staticAP_Compass_HIL compass;staticAP_Baro_HIL barometer;

//neverused#ifCONFIG_HAL_BOARD==HAL_BOARD_AVR_SITL//WhenbuildingforSITLweusetheHILbarometerandcompassdriversstaticSITLsitl;#endif#else#errorUnrecognisedHIL_MODEsetting.#endif//HILMODE////////////////////////////////////////////////////////////////////////////////Opticalflowsensor////////////////////////////////////////////////////////////////////////////////#ifOPTFLOW==ENABLEDstaticAP_OpticalFlow_ADNS3080optflow;#elsestaticAP_OpticalFlowoptflow;#endif//////////////////////////////////////////////////