APM飞控程序解读.doc

《APM飞控程序解读.doc》由会员分享，可在线阅读，更多相关《APM飞控程序解读.doc（36页珍藏版）》请在三一办公上搜索。

1、-/ -*- 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 version

2、 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. You shoul

3、d 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, Justin Beech,

4、 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 Declination

5、* 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 suggestion * HappyK

6、illmore: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 * Ma* Levine:Tri Support, Graphics * Jack Dunkle:Alp

7、ha 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 support * Olivier Adler :PPM E

8、ncoder * 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 mrela* version of Arduino, which can be found here: * :/code.google./p/ardupilot-mega/downloads/list * */ Header inc

9、ludes/#include #include #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 Mem

10、ory Library#include / ArduPilot Mega Analog to Digital Converter Library#include #include #include / ArduPilot Mega Magnetometer Library#include / ArduPilot Mega Vector/Matri* math Library#include / Curve used to linearlise throttle pwm to thrust#include / ArduPilot Mega Inertial Sensor (accel & gyr

11、o) Library#include #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 moun

12、t#include / needed 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

13、the loop support#include / 主循环调度程序#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 module

14、s#include Parameters.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

15、top to cooperate 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;/ protot

16、ypes/static void 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_S

17、ITL/static DataFlash_File DataFlash(/tmp/APMlogs);static DataFlash_SITL DataFlash;#elif CONFIG_HAL_BOARD = HAL_BOARD_P*4static DataFlash_File DataFlash(/fs/microsd/APM/logs);#elif CONFIG_HAL_BOARD = HAL_BOARD_LINU*static DataFlash_File DataFlash(logs);#elsestatic DataFlash_Empty DataFlash;#endif/运行主

18、循环/静态常量AP_InertialSensor: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 infor

19、mation directly./ - 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_MO

20、DE = HIL_MODE_DISABLED #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_InertialSe

21、nsor_HIL ins;#elif CONFIG_IMU_TYPE = CONFIG_IMU_P*4static AP_InertialSensor_P*4 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

22、 SITL we use the 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_P*4static AP_B

23、aro_P*4 barometer; #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

24、 setting. #endif #endif #if CONFIG_HAL_BOARD = HAL_BOARD_P*4static AP_Compass_P*4 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; #

25、elif GPS_PROTOCOL = GPS_PROTOCOL_SIRFAP_GPS_SIRF g_gps_driver; #elif GPS_PROTOCOL = GPS_PROTOCOL_UBLO*AP_GPS_UBLO* g_gps_driver; #elif GPS_PROTOCOL = GPS_PROTOCOL_MTKAP_GPS_MTK g_gps_driver; #elif GPS_PROTOCOL = GPS_PROTOCOL_MTK19AP_GPS_MTK19 g_gps_driver; #elif GPS_PROTOCOL = GPS_PROTOCOL_NONEAP_GP

26、S_None g_gps_driver; #else #error Unrecognised GPS_PROTOCOL setting. #endif / GPS PROTOCOLstatic AP_AHRS_DCM ahrs(&ins, g_gps);#elif HIL_MODE = HIL_MODE_SENSORS/ sensor emulatorsstatic AP_ADC_HIL adc;static AP_Baro_HIL barometer;static AP_Compass_HIL compass;static AP_GPS_HIL g_gps_driver;static AP_

27、InertialSensor_HIL ins;static AP_AHRS_DCM ahrs(&ins, g_gps); #if CONFIG_HAL_BOARD = HAL_BOARD_AVR_SITL / When building for SITL we use the HIL barometer and compass driversstatic SITL sitl;#endif#elif HIL_MODE = HIL_MODE_ATTITUDEstatic AP_ADC_HIL adc;static AP_InertialSensor_HIL ins;static AP_AHRS_H

28、IL ahrs(&ins, g_gps);static AP_GPS_HIL g_gps_driver;static AP_Compass_HIL compass; / never usedstatic AP_Baro_HIL barometer;#if CONFIG_HAL_BOARD = HAL_BOARD_AVR_SITL / When building for SITL we use the HIL barometer and compass driversstatic SITL sitl;#endif#else #error Unrecognised HIL_MODE setting

29、.#endif / HIL MODE/ Optical flow sensor/ #if OPTFLOW = ENABLEDstatic AP_OpticalFlow_ADNS3080 optflow; #elsestatic AP_OpticalFlow optflow; #endif/ GCS selection/static GCS_MAVLINK gcs0;static GCS_MAVLINK gcs3;/ SONAR selection/ModeFilterInt16_Size3 sonar_mode_filter(1);#if CONFIG_SONAR = ENABLEDstati

30、c AP_HAL:AnalogSource *sonar_analog_source;static AP_RangeFinder_Ma*sonar*L *sonar;#endif/ User variables/#ifdef USERHOOK_VARIABLES #include USERHOOK_VARIABLES#endif/ Global variables/* Radio values * Channel assignments * 1Ailerons (rudder if no ailerons) * 2Elevator * 3Throttle * 4Rudder (if we ha

31、ve ailerons) * 5Mode - 3 position switch * 6 User assignable * 7trainer switch - sets throttle nominal (toggle switch), sets accels to Level (hold 1 second) * 8TBD * Each Au* channel can be configured to have any of the available au*iliary functions assigned to it. * See libraries/RC_Channel/RC_Chan

32、nel_au*.h for more information */Documentation of GLobals:static union struct uint8_t home_is_set : 1; / 0 uint8_t simple_mode : 2; / 1,2 / This is the state of simple mode : 0 = disabled ; 1 = SIMPLE ; 2 = SUPERSIMPLE uint8_t pre_arm_rc_check : 1; / 3 / true if rc input pre-arm checks have been com

33、pleted successfully uint8_t pre_arm_check : 1; / 4 / true if all pre-arm checks (rc, accel calibration, gps lock) have been performed uint8_t auto_armed : 1; / 5 / stops auto missions from beginning until throttle is raised uint8_t logging_started : 1; / 6 / true if dataflash logging has started uin

34、t8_t do_flip : 1; / 7 / Used to enable flip code uint8_t takeoff_complete : 1; / 8 uint8_t land_complete : 1; / 9 / true if we have detected a landing uint8_t new_radio_frame : 1; / 10 / Set true if we have new PWM data to act on from the Radio uint8_t CH7_flag : 2; / 11,12 / ch7 au* switch : 0 is low or false, 1 is center or true, 2 is high uint8_t CH8_flag : 2; / 13,14 / ch8 au* switch : 0 is low or