APM飞控程序解读.docx

1、APM飞控程序解读/ -*- tab-width: 4; Mode: C+; c-basic-offset: 4; indent-tabs-mode: nil -*-#define THISFIRMWARE ArduCopter V3.1-rc5/* This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either

2、 version 3 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. Y

3、ou should have received a copy of the GNU General Public License along with this program. If not, see . */* * ArduCopter Version 3.0 * Creator: Jason Short * Lead Developer: Randy Mackay * Based on code and ideas from the Arducopter team: Pat Hickey, Jose Julio, Jani Hirvinen, Andrew Tridgell, Justi

4、n Beech, Adam Rivera, Jean-Louis Naudin, Roberto Navoni * Thanks to: Chris Anderson, Mike Smith, Jordi Munoz, Doug Weibel, James Goppert, Benjamin Pelletier, Robert Lefebvre, Marco Robustini * * Special Thanks for Contributors (in alphabetical order by first name): * * Adam M Rivera :Auto Compass De

5、clination * Amilcar Lucas :Camera mount library * Andrew Tridgell :General development, Mavlink Support * Angel Fernandez :Alpha testing * Doug Weibel :Libraries * Christof Schmid :Alpha testing * Dani Saez :V Octo Support * Gregory Fletcher :Camera mount orientation math * Guntars :Arming safety su

6、ggestion * HappyKillmore :Mavlink GCS * Hein Hollander :Octo Support * Igor van Airde :Control Law optimization * Leonard Hall :Flight Dynamics, Throttle, Loiter and Navigation Controllers * Jonathan Challinger :Inertial Navigation * Jean-Louis Naudin :Auto Landing * Max Levine :Tri Support, Graphic

7、s * Jack Dunkle :Alpha testing * James Goppert :Mavlink Support * Jani Hiriven :Testing feedback * John Arne Birkeland :PPM Encoder * Jose Julio :Stabilization Control laws * Marco Robustini :Lead tester * Michael Oborne :Mission Planner GCS * Mike Smith :Libraries, Coding support * Oliver :Piezo su

8、pport * Olivier Adler :PPM Encoder * Robert Lefebvre :Heli Support & LEDs * Sandro Benigno :Camera support * * And much more so PLEASE PM me on DIYDRONES to add your contribution to the List * * Requires modified mrelax version of Arduino, which can be found here: * * */ Header includes/#include #in

9、clude #include / Common dependencies#include #include #include #include / AP_HAL#include #include #include #include #include #include #include / Application dependencies#include / MAVLink GCS定义#include / ArduPilot GPS library#include / 全球定位系统干扰保护库#include / ArduPilot Mega Flash Memory Library#includ

10、e / ArduPilot Mega Analog to Digital Converter Library#include #include #include / ArduPilot Mega Magnetometer Library#include / ArduPilot Mega Vector/Matrix math Library#include / Curve used to linearlise throttle pwm to thrust#include / ArduPilot Mega Inertial Sensor (accel & gyro) Library#include

11、 #include / PI library#include / PID library#include /遥控通道库#include / AP Motors library#include / Range finder library#include / Optical Flow library#include / Filter library#include / APM FIFO Buffer#include / APM relay#include / Photo or video camera#include / Camera/Antenna mount#include / needed

12、 for AHRS build#include / needed for AHRS build#include / ArduPilot Mega inertial 导航 library#include / ArduCopter waypoint navigation library#include / ArduPilot Mega Declination Helper Library#include / Arducopter Fence library#include / memory limit checker#include / software in the loop support#i

13、nclude / 主循环调度程序#include / RC input mapping library#include / Notify library#include / Battery monitor library#if SPRAYER = ENABLED#include / crop sprayer library#endif/ AP_HAL Arduino兼容性层#include compat.h/配置#include defines.h#include config.h#include config_channels.h/ Local modules#include Paramet

14、ers.h#include GCS.h/ cliSerial/ cliSerial isnt strictly necessary - it is an alias for hal.console. It may/ be deprecated in favor of hal.console in later releases.static AP_HAL:BetterStream* cliSerial;/ N.B. we need to keep a static declaration which isnt guarded by macros/ at the top to cooperate

15、with the prototype mangler. / AP_HAL instance/const AP_HAL:HAL& hal = AP_HAL_BOARD_DRIVER;/ Parameters/ Global parameters are all contained within the g class./static Parameters g;/ main loop schedulerstatic AP_Scheduler scheduler;/ AP_Notify instancestatic AP_Notify notify;/ prototypes/static void

16、update_events(void);static void print_flight_mode(AP_HAL:BetterStream *port, uint8_t mode);/ Dataflash/#if CONFIG_HAL_BOARD = HAL_BOARD_APM2static DataFlash_APM2 DataFlash;#elif CONFIG_HAL_BOARD = HAL_BOARD_APM1static DataFlash_APM1 DataFlash;#elif CONFIG_HAL_BOARD = HAL_BOARD_AVR_SITL/static DataFl

17、ash_File DataFlash(/tmp/APMlogs);static DataFlash_SITL DataFlash;#elif CONFIG_HAL_BOARD = HAL_BOARD_PX4static DataFlash_File DataFlash(/fs/microsd/APM/logs);#elif CONFIG_HAL_BOARD = HAL_BOARD_LINUXstatic DataFlash_File DataFlash(logs);#elsestatic DataFlash_Empty DataFlash;#endif/运行主循环/静态常量AP_Inertia

18、lSensor:Sample_rate ins_sample_rate = AP_InertialSensor:RATE_100HZ;/ Sensors/ There are three basic options related to flight sensor selection./ - Normal flight mode. Real sensors are used./ - HIL Attitude mode. Most sensors are disabled, as the HIL/ protocol supplies attitude information directly./

19、 - HIL Sensors mode. Synthetic sensors are configured that/ supply data from the simulation./ All GPS access should be through this pointer.static GPS *g_gps;static GPS_Glitch gps_glitch(g_gps);/ flight modes convenience arraystatic AP_Int8 *flight_modes = &g.flight_mode1;#if HIL_MODE = HIL_MODE_DIS

20、ABLED #if CONFIG_ADC = ENABLEDstatic AP_ADC_ADS7844 adc; #endif #if CONFIG_IMU_TYPE = CONFIG_IMU_MPU6000static AP_InertialSensor_MPU6000 ins;#elif CONFIG_IMU_TYPE = CONFIG_IMU_OILPANstatic AP_InertialSensor_Oilpan ins(&adc);#elif CONFIG_IMU_TYPE = CONFIG_IMU_SITLstatic AP_InertialSensor_HIL ins;#eli

21、f CONFIG_IMU_TYPE = CONFIG_IMU_PX4static AP_InertialSensor_PX4 ins;#elif CONFIG_IMU_TYPE = CONFIG_IMU_FLYMAPLEAP_InertialSensor_Flymaple ins;#elif CONFIG_IMU_TYPE = CONFIG_IMU_L3G4200DAP_InertialSensor_L3G4200D ins;#endif #if CONFIG_HAL_BOARD = HAL_BOARD_AVR_SITL / When building for SITL we use the

22、HIL barometer and compass driversstatic AP_Baro_HIL barometer;static AP_Compass_HIL compass;static SITL sitl; #else/ Otherwise, instantiate a real barometer and compass driver #if CONFIG_BARO = AP_BARO_BMP085static AP_Baro_BMP085 barometer; #elif CONFIG_BARO = AP_BARO_PX4static AP_Baro_PX4 barometer

23、; #elif CONFIG_BARO = AP_BARO_MS5611 #if CONFIG_MS5611_SERIAL = AP_BARO_MS5611_SPIstatic AP_Baro_MS5611 barometer(&AP_Baro_MS5611:spi); #elif CONFIG_MS5611_SERIAL = AP_BARO_MS5611_I2Cstatic AP_Baro_MS5611 barometer(&AP_Baro_MS5611:i2c); #else #error Unrecognized CONFIG_MS5611_SERIAL setting. #endif

24、#endif #if CONFIG_HAL_BOARD = HAL_BOARD_PX4static AP_Compass_PX4 compass; #elsestatic AP_Compass_HMC5843 compass; #endif #endif/ real GPS selection #if GPS_PROTOCOL = GPS_PROTOCOL_AUTOAP_GPS_Auto g_gps_driver(&g_gps); #elif GPS_PROTOCOL = GPS_PROTOCOL_NMEAAP_GPS_NMEA g_gps_driver; #elif GPS_PROTOCOL = GPS_PROTOCOL_SIRFAP_GPS_SIRF g_gps_driver; #elif GPS_PROTOCOL = GPS_P