APM飞控程序解读[精校版本]

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 should

3、 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 * HappyKi

6、llmore: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, Graphics * Jack Dunkle:Alph

7、a 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 En

8、coder * 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: * /p/ardupilot-mega/downloads/list * */ Head

9、er includes/#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 Fla

10、sh Memory Library#include / 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

11、 & gyro) 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/Antenn

12、a mount#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 / softwa

13、re in 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

14、 modules#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/

15、at the 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;/

16、 prototypes/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_BOAR

17、D_AVR_SITL/static DataFlash_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;#en

18、dif/运行主循环/静态常量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 attitud

19、e information 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

20、 HIL_MODE = 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_Ine

21、rtialSensor_HIL ins;#elif 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 build

22、ing for 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_PX4stat

23、ic AP_Baro_PX4 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

24、_SERIAL setting. #endif #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_dr

25、iver; #elif GPS_PROTOCOL = GPS_PROTOCOL_SIRFAP_GPS_SIRF g_gps_driver; #elif GPS_PROTOCOL = GPS_PROTOCOL_UBLOXAP_GPS_UBLOX 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_NO

26、NEAP_GPS_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;sta

27、tic AP_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

28、_AHRS_HIL 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

29、setting.#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 = ENABL

30、EDstatic AP_HAL:AnalogSource *sonar_analog_source;static AP_RangeFinder_MaxsonarXL *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 (i

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

32、RC_Channel_aux.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 b

33、een completed 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 star

34、ted uint8_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 aux switch :

35、 0 is low or false, 1 is center or true, 2 is high uint8_t CH8_flag : 2; / 13,14 / ch8 aux switch : 0 is low or false, 1 is center or true, 2 is high uint8_t usb_connected : 1; / 15 / true if APM is powered from USB connection uint8_t yaw_stopped : 1; / 16 / Used to manage the Yaw hold capabilities

36、uint8_t disable_stab_rate_limit : 1; / 17 / disables limits rate request from the stability controller uint8_t rc_receiver_present : 1; / 18 / true if we have an rc receiver present (i.e. if weve ever received an update ; uint32_t value; ap;/ Radio/ This is the state of the flight control system/ Th

37、ere are multiple states defined such as STABILIZE, ACRO,static int8_t control_mode = STABILIZE;/ Used to maintain the state of the previous control switch position/ This is set to -1 when we need to re-read the switchstatic uint8_t oldSwitchPosition;static RCMapper rcmap;/ receiver RSSIstatic uint8_

38、t receiver_rssi;/ Failsafe/static struct uint8_t rc_override_active : 1; / 0 / true if rc control are overwritten by ground station uint8_t radio : 1; / 1 / A status flag for the radio failsafe uint8_t battery : 1; / 2 / A status flag for the battery failsafe uint8_t gps : 1; / 3 / A status flag for

39、 the gps failsafe uint8_t gcs : 1; / 4 / A status flag for the ground station failsafe int8_t radio_counter; / number of iterations with throttle below throttle_fs_value uint32_t last_heartbeat_ms; / the time when the last HEARTBEAT message arrived from a GCS - used for triggering gcs failsafe fails

40、afe;/ Motor Output/#if FRAME_CONFIG = QUAD_FRAME #define MOTOR_CLASS AP_MotorsQuad#elif FRAME_CONFIG = TRI_FRAME #define MOTOR_CLASS AP_MotorsTri#elif FRAME_CONFIG = HEXA_FRAME #define MOTOR_CLASS AP_MotorsHexa#elif FRAME_CONFIG = Y6_FRAME #define MOTOR_CLASS AP_MotorsY6#elif FRAME_CONFIG = OCTA_FRA

41、ME #define MOTOR_CLASS AP_MotorsOcta#elif FRAME_CONFIG = OCTA_QUAD_FRAME #define MOTOR_CLASS AP_MotorsOctaQuad#elif FRAME_CONFIG = HELI_FRAME #define MOTOR_CLASS AP_MotorsHeli#else #error Unrecognised frame type#endif#if FRAME_CONFIG = HELI_FRAME / helicopter constructor requires more argumentsstatic MOTOR_CLASS motors(&g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, &g.rc_8, &g.heli_servo_1, &g.heli_servo_2, &g.