/// -*- 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
// Common dependencies #include #include #include #include // AP_HAL
#include
#include
#include #include
#include #include #include
// Application dependencies
#include
#include
#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
#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
#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;
#elif CONFIG_BARO == AP_BARO_PX4 static AP_Baro_PX4 barometer;
#elif CONFIG_BARO == AP_BARO_MS5611
#if CONFIG_MS5611_SERIAL == AP_BARO_MS5611_SPI
static AP_Baro_MS5611 barometer(&AP_Baro_MS5611::spi); #elif CONFIG_MS5611_SERIAL == AP_BARO_MS5611_I2C static AP_Baro_MS5611 barometer(&AP_Baro_MS5611::i2c); #else
#error Unrecognized CONFIG_MS5611_SERIAL setting. #endif #endif
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 static AP_Compass_PX4 compass; #else
static AP_Compass_HMC5843 compass; #endif #endif
// real GPS selection
#if GPS_PROTOCOL == GPS_PROTOCOL_AUTO AP_GPS_Auto g_gps_driver(&g_gps);
#elif GPS_PROTOCOL == GPS_PROTOCOL_NMEA AP_GPS_NMEA g_gps_driver;
#elif GPS_PROTOCOL == GPS_PROTOCOL_SIRF AP_GPS_SIRF g_gps_driver;
#elif GPS_PROTOCOL == GPS_PROTOCOL_UBLOX AP_GPS_UBLOX g_gps_driver;
#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK AP_GPS_MTK g_gps_driver;
#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK19 AP_GPS_MTK19 g_gps_driver;
#elif GPS_PROTOCOL == GPS_PROTOCOL_NONE AP_GPS_None g_gps_driver;
#else
#error Unrecognised GPS_PROTOCOL setting. #endif // GPS PROTOCOL
static AP_AHRS_DCM ahrs(&ins, g_gps);
#elif HIL_MODE == HIL_MODE_SENSORS // sensor emulators
static 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 drivers static SITL sitl; #endif