APM飞控程序解读.doc

/ -*- 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 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 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, 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 * 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 * 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, Graphics * 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 support * 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: * /p/ardupilot-mega/downloads/list * */ Header 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 Flash 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 & 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/Antenna 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 / software 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 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/ 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;/ 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_BOARD_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;#endif/运行主循环/静态常量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 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 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_InertialSensor_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 building 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_PX4static 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_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_driver; #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_NONEAP_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;static 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_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 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 = ENABLEDstatic 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 (if 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/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 been 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 started 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 : 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 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/ There 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_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 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 failsafe;/ 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_FRAME #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.he