APM飞控程序解读

/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-

#define THISFIRMWARE \/*

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 \ * http://code.google.com/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 definitions #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 to Arduino compatibility layer #include \// Configuration #include \#include \

#include \

// Local modules

#include \#include \

//////////////////////////////////////////////////////////////////////////////// // 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 scheduler

static AP_Scheduler scheduler;

// AP_Notify instance static 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_APM2 static DataFlash_APM2 DataFlash;

#elif CONFIG_HAL_BOARD == HAL_BOARD_APM1 static DataFlash_APM1 DataFlash;

#elif CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL

//static DataFlash_File DataFlash(\static DataFlash_SITL DataFlash;

#elif CONFIG_HAL_BOARD == HAL_BOARD_PX4

static DataFlash_File DataFlash(\#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX static DataFlash_File DataFlash(\#else

static DataFlash_Empty DataFlash; #endif

//////////////////////////////////////////////////////////////////////////////// // the rate we run the main loop at

////////////////////////////////////////////////////////////////////////////////

static const 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 array

static AP_Int8 *flight_modes = &g.flight_mode1;

#if HIL_MODE == HIL_MODE_DISABLED

#if CONFIG_ADC == ENABLED static AP_ADC_ADS7844 adc; #endif

#if CONFIG_IMU_TYPE == CONFIG_IMU_MPU6000 static AP_InertialSensor_MPU6000 ins;

#elif CONFIG_IMU_TYPE == CONFIG_IMU_OILPAN static AP_InertialSensor_Oilpan ins(&adc); #elif CONFIG_IMU_TYPE == CONFIG_IMU_SITL static AP_InertialSensor_HIL ins;

#elif CONFIG_IMU_TYPE == CONFIG_IMU_PX4 static AP_InertialSensor_PX4 ins;

#elif CONFIG_IMU_TYPE == CONFIG_IMU_FLYMAPLE AP_InertialSensor_Flymaple ins;

#elif CONFIG_IMU_TYPE == CONFIG_IMU_L3G4200D AP_InertialSensor_L3G4200D ins; #endif

#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL

// When building for SITL we use the HIL barometer and compass drivers static 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_BMP085 static AP_Baro_BMP085 barometer;