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 <.gnu.org/licenses/>. */* * 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 * Ma* 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 "mrela*" version of Arduino, which can be found here: * :/code.google./p/ardupilot-mega/downloads/list * */ Header includes/#include <math.h>#include <stdio.h>#include <stdarg.h>/ Common dependencies#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_P*4.h>#include <AP_HAL_FLYMAPLE.h>#include <AP_HAL_Linu*.h>#include <AP_HAL_Empty.h>/ Application dependencies#include <GCS_MAVLink.h> / MAVLink GCS定义#include <AP_GPS.h> / ArduPilot GPS library#include <AP_GPS_Glitch.h> / 全球定位系统干扰保护库#include <DataFlash.h> / ArduPilot Mega Flash Memory Library#include <AP_ADC.h> / ArduPilot Mega Analog to Digital Converter Library#include <AP_ADC_AnalogSource.h>#include <AP_Baro.h>#include <AP_Compass.h> / ArduPilot Mega Magnetometer Library#include <AP_Math.h> / ArduPilot Mega Vector/Matri* math Library#include <AP_Curve.h> / Curve used to linearlise throttle pwm to thrust#include <AP_InertialSensor.h> / ArduPilot Mega Inertial Sensor (accel & gyro) Library#include <AP_AHRS.h>#include <APM_PI.h> / PI library#include <AC_PID.h> / PID library#include <RC_Channel.h> /遥控通道库#include <AP_Motors.h> / AP Motors library#include <AP_RangeFinder.h> / Range finder library#include <AP_OpticalFlow.h> / Optical Flow library#include <Filter.h> / Filter library#include <AP_Buffer.h> / APM FIFO Buffer#include <AP_Relay.h> / APM relay#include <AP_Camera.h> / Photo or video camera#include <AP_Mount.h> / Camera/Antenna mount#include <AP_Airspeed.h> / needed for AHRS build#include <AP_Vehicle.h> / needed for AHRS build#include <AP_InertialNav.h> / ArduPilot Mega inertial 导航 library#include <AC_WPNav.h> / ArduCopter waypoint navigation library#include <AP_Declination.h> / ArduPilot Mega Declination Helper Library#include <AC_Fence.h> / Arducopter Fence library#include <memcheck.h> / memory limit checker#include <SITL.h> / software in the loop support#include <AP_Scheduler.h> / 主循环调度程序#include <AP_RCMapper.h> / RC input mapping library#include <AP_Notify.h> / Notify library#include <AP_BattMonitor.h> / Battery monitor library#if SPRAYER = ENABLED#include <AC_Sprayer.h> / 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 isn't 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 isn't 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_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/运行主循环/静态常量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_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 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_Baro_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 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; #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_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_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 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 Au* channel can be configured to have any of the available au*iliary functions assigned to it. * See libraries/RC_Channel/RC_Channel_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 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 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