APM飞控源码讲解
APM飞控源码讲解
APM飞控系统介绍APM飞控系统是国外的一个开源飞控系统，能够支持固定翼，直升机，3轴，4轴，6轴飞行器。
在此我只介绍固定翼飞控系统。
飞控原理在APM飞控系统中，采用的是两级PID控制方式，第一级是导航级，第二级是控制级，导航级的计算集中在medium_loop( ) 和fastloop( )的update_current_flight_mode( )函数中，控制级集中在fastloop( )的stabilize( )函数中。
导航级PID控制就是要解决飞机如何以预定空速飞行在预定高度的问题，以及如何转弯飞往目标问题，通过算法给出飞机需要的俯仰角、油门和横滚角，然后交给控制级进行控制解算。
控制级的任务就是依据需要的俯仰角、油门、横滚角，结合飞机当前的姿态解算出合适的舵机控制量，使飞机保持预定的俯仰角，横滚角和方向角。
最后通过舵机控制级set_servos_4( )将控制量转换成具体的pwm信号量输出给舵机。
值得一提的是，油门的控制量是在导航级确定的。
控制级中不对油门控制量进行解算，而直接交给舵机控制级。
而对于方向舵的控制，导航级并不给出方向舵量的解算，而是由控制级直接解算方向舵控制量，然后再交给舵机控制级。
以下，我剔除了APM飞控系统的细枝末节，仅仅将飞控系统的重要语句展现，只浅显易懂地说明APM飞控系统的核心工作原理。
一，如何让飞机保持预定高度和空速飞行要想让飞机在预定高度飞行，飞控必须控制好飞机的升降舵和油门，因此，首先介绍固定翼升降舵和油门的控制，固定翼的升降舵和油门控制方式主要有两种：一种是高度控制油门，空速控制升降舵方式。
实际飞行存在四种情况，第一种情况是飞机飞行过程中，如果高度低于目标高度，飞控就会控制油门加大，从而导致空速加大，然后才导致拉升降舵，飞机爬升；第二种情况与第一种情况相反；第三种情况是飞机在目标高度，但是空速高于目标空速，这种情况飞控会直接拉升降舵，使飞机爬升，降低空速，但是，高度增加了，飞控又会减小油门，导致空速降低，空速低于目标空速后，飞控推升降舵，导致飞机降低高度。
APM飞控程序解读
/// -*- 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 modifyit under the terms of the GNU General Public License as published bythe 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 ofMERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See theGNU General Public License for more details.You should have received a copy of the GNU General Public Licensealong with this program. If not, see </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* 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 <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_PX4.h>#include <AP_HAL_FLYMAPLE.h>#include <AP_HAL_Linux.h>#include <AP_HAL_Empty.h>// Application dependencies#include <GCS_MAVLink.h> // MAVLink GCS definitions#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/Matrix 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// AP_HAL to Arduino compatibility layer#include "compat.h"// Configuration#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_PX4static DataFlash_File DataFlash("/fs/microsd/APM/logs");#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUXstatic DataFlash_File DataFlash("logs");#elsestatic 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 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 drivers static 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* 1 Ailerons (rudder if no ailerons)* 2 Elevator* 3 Throttle* 4 Rudder (if we have ailerons)* 5 Mode - 3 position switch* 6 User assignable* 7 trainer switch - sets throttle nominal (toggle switch), sets accels to Level (hold >1 second)* 8 TBD* 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; // 0uint8_t simple_mode : 2; // 1,2 // This is the state of simple mode : 0 = disabled ; 1 = SIMPLE ;2 = SUPERSIMPLEuint8_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 performeduint8_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 starteduint8_t do_flip : 1; // 7 // Used to enable flip codeuint8_t takeoff_complete : 1; // 8uint8_t land_complete : 1; // 9 // true if we have detected a landinguint8_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 highuint8_t CH8_flag : 2; // 13,14 // ch8 aux switch : 0 is low or false, 1 is center or true, 2 is highuint8_t usb_connected : 1; // 15 // true if APM is powered from USB connectionuint8_t yaw_stopped : 1; // 16 // Used to manage the Yaw hold capabilitiesuint8_t disable_stab_rate_limit : 1; // 17 // disables limits rate request from the stability controlleruint8_t rc_receiver_present : 1; // 18 // true if we have an rc receiver present (i.e. if we've 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 stationuint8_t radio : 1; // 1 // A status flag for the radio failsafeuint8_t battery : 1; // 2 // A status flag for the battery failsafeuint8_t gps : 1; // 3 // A status flag for the gps failsafeuint8_t gcs : 1; // 4 // A status flag for the ground station failsafeint8_t radio_counter; // number of iterations with throttle below throttle_fs_valueuint32_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.heli_servo_3, &g.heli_servo_4);#elif FRAME_CONFIG == TRI_FRAME // tri constructor requires additional rc_7 argument to allow tail servo reversing static MOTOR_CLASS motors(&g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, &g.rc_7);#elsestatic MOTOR_CLASS motors(&g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4);#endif////////////////////////////////////////////////////////////////////////////////// PIDs////////////////////////////////////////////////////////////////////////////////// This is a convienience accessor for the IMU roll rates. It's currently the raw IMU rates// and not the adjusted omega rates, but the name is stuckstatic Vector3f omega;// This is used to hold radio tuning values for in-flight CH6 tuningfloat tuning_value;// used to limit the rate that the pid controller output is logged so that it doesn't negatively affect performance static uint8_t pid_log_counter;////////////////////////////////////////////////////////////////////////////////// LED output////////////////////////////////////////////////////////////////////////////////// Blinking indicates GPS statusstatic uint8_t copter_leds_GPS_blink;// Blinking indicates battery statusstatic uint8_t copter_leds_motor_blink;// Navigation confirmation blinksstatic int8_t copter_leds_nav_blink;////////////////////////////////////////////////////////////////////////////////// GPS variables////////////////////////////////////////////////////////////////////////////////// This is used to scale GPS values for EEPROM storage// 10^7 times Decimal GPS means 1 == 1cm// This approximation makes calculations integer and it's easy to readstatic const float t7 = 10000000.0;// We use atan2 and other trig techniques to calaculate angles// We need to scale the longitude up to make these calcs work// to account for decreasing distance between lines of longitude away from the equatorstatic float scaleLongUp = 1;// Sometimes we need to remove the scaling for distance calcsstatic float scaleLongDown = 1;////////////////////////////////////////////////////////////////////////////////// Location & Navigation////////////////////////////////////////////////////////////////////////////////// This is the angle from the copter to the next waypoint in centi-degreesstatic int32_t wp_bearing;// The original bearing to the next waypoint. used to point the nose of the copter at the next waypoint static int32_t original_wp_bearing;// The location of home in relation to the copter in centi-degreesstatic int32_t home_bearing;// distance between plane and home in cmstatic int32_t home_distance;// distance between plane and next waypoint in cm.static uint32_t wp_distance;// navigation mode - options include NAV_NONE, NAV_LOITER, NAV_CIRCLE, NAV_WPstatic uint8_t nav_mode;// Register containing the index of the current navigation command in the mission scriptstatic int16_t command_nav_index;// Register containing the index of the previous navigation command in the mission script// Used to manage the execution of conditional commandsstatic uint8_t prev_nav_index;// Register containing the index of the current conditional command in the mission scriptstatic uint8_t command_cond_index;// Used to track the required WP navigation information// options include// NAV_ALTITUDE - have we reached the desired altitude?// NAV_LOCATION - have we reached the desired location?// NAV_DELAY - have we waited at the waypoint the desired time?static float lon_error, lat_error; // Used to report how many cm we are from the next waypoint or loiter target positionstatic int16_t control_roll;static int16_t control_pitch;static uint8_t rtl_state; // records state of rtl (initial climb, returning home, etc)static uint8_t land_state; // records state of land (flying to location, descending)////////////////////////////////////////////////////////////////////////////////// Orientation（方向）////////////////////////////////////////////////////////////////////////////////// Convienience accessors for commonly used trig functions. These values are generated// by the DCM through a few simple equations. They are used throughout the code where cos and sin// would normally be used.// The cos values are defaulted to 1 to get a decent initial value for a level statestatic float cos_roll_x = 1.0;static float cos_pitch_x = 1.0;static float cos_yaw = 1.0;static float sin_yaw;static float sin_roll;static float sin_pitch;////////////////////////////////////////////////////////////////////////////////// SIMPLE Mode////////////////////////////////////////////////////////////////////////////////// Used to track the orientation of the copter for Simple mode. This value is reset at each arming// or in SuperSimple mode when the copter leaves a 20m radius from home.static float simple_cos_yaw = 1.0;static float simple_sin_yaw;static int32_t super_simple_last_bearing;static float super_simple_cos_yaw = 1.0;static float super_simple_sin_yaw;// Stores initial bearing when armed - initial simple bearing is modified in super simple mode so not suitable static int32_t initial_armed_bearing;////////////////////////////////////////////////////////////////////////////////// Rate contoller targets////////////////////////////////////////////////////////////////////////////////static uint8_t rate_targets_frame = EARTH_FRAME; // indicates whether rate targets provided in earth or body framestatic int32_t roll_rate_target_ef;static int32_t pitch_rate_target_ef;static int32_t yaw_rate_target_ef;static int32_t roll_rate_target_bf; // body frame roll rate targetstatic int32_t pitch_rate_target_bf; // body frame pitch rate targetstatic int32_t yaw_rate_target_bf; // body frame yaw rate target////////////////////////////////////////////////////////////////////////////////// Throttle（风门，调节） variables////////////////////////////////////////////////////////////////////////////////static int16_t throttle_accel_target_ef; // earth frame throttle acceleration targetstatic bool throttle_accel_controller_active; // true when accel based throttle controller is active, false when higher level throttle controllers are providing throttle output directlystatic float throttle_avg; // g.throttle_cruise as a floatstatic int16_t desired_climb_rate; // pilot desired climb rate - for logging purposes onlystatic float target_alt_for_reporting; // target altitude in cm for reporting (logs and ground station)////////////////////////////////////////////////////////////////////////////////// ACRO（高） Mode////////////////////////////////////////////////////////////////////////////////// Used to control Axis lockstatic int32_t acro_roll; // desired roll angle while sport modestatic int32_t acro_roll_rate; // desired roll rate while in acro modestatic int32_t acro_pitch; // desired pitch angle while sport modestatic int32_t acro_pitch_rate; // desired pitch rate while acro modestatic int32_t acro_yaw_rate; // desired yaw rate while acro modestatic float acro_level_mix; // scales back roll, pitch and yaw inversely proportional to input from pilot// Filters#if FRAME_CONFIG == HELI_FRAME//static LowPassFilterFloat rate_roll_filter; // Rate Roll filter//static LowPassFilterFloat rate_pitch_filter; // Rate Pitch filter#endif // HELI_FRAME////////////////////////////////////////////////////////////////////////////////// Circle Mode / Loiter（走停） control////////////////////////////////////////////////////////////////////////////////Vector3f circle_center; // circle position expressed in cm from home location. x = lat, y = lon// angle from the circle center to the copter's desired location. Incremented at circle_rate / secondstatic float circle_angle;// the total angle (in radians) travelledstatic float circle_angle_total;// deg : how many times to circle as specified by mission commandstatic uint8_t circle_desired_rotations;static float circle_angular_acceleration; // circle mode's angular accelerationstatic float circle_angular_velocity; // circle mode's angular velocitystatic float circle_angular_velocity_max; // circle mode's max angular velocity// How long we should stay in Loiter Mode for mission scripting (time in seconds)static uint16_t loiter_time_max;// How long have we been loitering - The start time in millisstatic uint32_t loiter_time;////////////////////////////////////////////////////////////////////////////////// CH7 and CH8 save waypoint（航路点） control////////////////////////////////////////////////////////////////////////////////// This register tracks the current Mission Command index when writing// a mission using Ch7 or Ch8 aux switches in flightstatic int8_t aux_switch_wp_index;////////////////////////////////////////////////////////////////////////////////// Battery Sensors////////////////////////////////////////////////////////////////////////////////static AP_BattMonitor battery;////////////////////////////////////////////////////////////////////////////////// Altitude////////////////////////////////////////////////////////////////////////////////// The (throttle) controller desired altitude in cmstatic float controller_desired_alt;// The cm we are off in altitude from next_WP.alt – Positive value means we are below the WP static int32_t altitude_error;// The cm/s we are moving up or down based on filtered data - Positive = UPstatic int16_t climb_rate;// The altitude as reported by Sonar in cm – Values are 20 to 700 generally.static int16_t sonar_alt;static uint8_t sonar_alt_health; // true if we can trust the altitude from the sonarstatic float target_sonar_alt; // desired altitude in cm above the ground// The altitude as reported by Baro in cm – Values can be quite highstatic int32_t baro_alt;static int16_t saved_toy_throttle;////////////////////////////////////////////////////////////////////////////////// flight modes////////////////////////////////////////////////////////////////////////////////// Flight modes are combinations of Roll/Pitch, Yaw and Throttle control modes// Each Flight mode is a unique combination of these modes//// The current desired control scheme for Yawstatic uint8_t yaw_mode;// The current desired control scheme for roll and pitch / navigationstatic uint8_t roll_pitch_mode;// The current desired control scheme for altitude holdstatic uint8_t throttle_mode;////////////////////////////////////////////////////////////////////////////////// flight specific////////////////////////////////////////////////////////////////////////////////// An additional throttle added to keep the copter at the same altitude when banking。
apm无人机主程序分析
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-#define THISFIRMWARE "ArduCopter V3.0.1"/** 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** This firmware is free software; you can redistribute it and/or* modify it under the terms of the GNU Lesser General Public* License as published by the Free Software Foundation; either* version 2.1 of the License, or (at your option) any later version.** 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 <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_A VR.h>#include <AP_HAL_A VR_SITL.h>#include <AP_HAL_SMACCM.h>#include <AP_HAL_PX4.h>#include <AP_HAL_Empty.h>// Application dependencies#include <GCS_MA VLink.h> // MA VLink GCS definitions#include <AP_GPS.h> // ArduPilot GPS library#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/Matrix 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> // RC Channel Library#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_InertialNav.h> // ArduPilot Mega inertial navigation 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> // main loop scheduler#include <AP_RCMapper.h> // RC input mapping library// AP_HAL to Arduino compatibility layer#include "compat.h"// Configuration#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 instanceconst 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;////////////////////////////////////////////////////////////////////////////////// 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_A VR_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");#elsestatic DataFlash_Empty DataFlash;#endif////////////////////////////////////////////////////////////////////////////////// the rate we run the main loop at////////////////////////////////////////////////////////////////////////////////static const AP_InertialSensor::Sample_rate ins_sample_rate = AP_InertialSensor::RATE_200HZ;// 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;// 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_Stub ins;#elif CONFIG_IMU_TYPE == CONFIG_IMU_PX4static AP_InertialSensor_PX4 ins;#endif#if CONFIG_HAL_BOARD == HAL_BOARD_A VR_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_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_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_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 PROTOCOL#if DMP_ENABLED == ENABLED && CONFIG_HAL_BOARD == HAL_BOARD_APM2 static AP_AHRS_MPU6000 ahrs(&ins, g_gps); // only works with APM2#elsestatic AP_AHRS_DCM ahrs(&ins, g_gps);#endif// ahrs2 object is the secondary ahrs to allow running DMP in parallel with DCM#if SECONDARY_DMP_ENABLED == ENABLED && CONFIG_HAL_BOARD == HAL_BOARD_APM2static AP_AHRS_MPU6000 ahrs2(&ins, g_gps); // only works with APM2#endif#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_Stub ins;static AP_AHRS_DCM ahrs(&ins, g_gps);static int32_t gps_base_alt;#if CONFIG_HAL_BOARD == HAL_BOARD_A VR_SITL// When building for SITL we use the HIL barometer and compass driversstatic SITL sitl;#endif#elif HIL_MODE == HIL_MODE_A TTITUDEstatic AP_ADC_HIL adc;static AP_InertialSensor_Stub 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;static int32_t gps_base_alt;#if CONFIG_HAL_BOARD == HAL_BOARD_A VR_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_MA VLINK gcs0;static GCS_MA VLINK 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_V ARIABLES#include USERHOOK_V ARIABLES#endif////////////////////////////////////////////////////////////////////////////////// Global variables/////////////////////////////////////////////////////////////////////////////////* Radio values* Channel assignments* 1 Ailerons (rudder if no ailerons) * 2 Elevator* 3 Throttle* 4 Rudder (if we have ailerons)* 5 Mode - 3 position switch* 6 User assignable* 7 trainer switch - sets throttle nominal (toggle switch), sets accels to Level (hold > 1 second)* 8 TBD* 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; // 0uint8_t simple_mode : 1; // 1 // This is the state of simple modeuint8_t manual_attitude : 1; // 2uint8_t manual_throttle : 1; // 3uint8_t pre_arm_rc_check : 1; // 5 // true if rc input pre-arm checks have been completed successfullyuint8_t pre_arm_check : 1; // 6 // true if all pre-arm checks (rc, accel calibration, gps lock) have been performeduint8_t auto_armed : 1; // 7 // stops auto missions from beginning until throttle is raiseduint8_t logging_started : 1; // 8 // true if dataflash logging has starteduint8_t low_battery : 1; // 9 // Used to track if the battery is low - LED output flashes when the batt is lowuint8_t failsafe_radio : 1; // 10 // A status flag for the radio failsafeuint8_t failsafe_batt : 1; // 11 // A status flag for the battery failsafeuint8_t failsafe_gps : 1; // 12 // A status flag for the gps failsafeuint8_t failsafe_gcs : 1; // 13 // A status flag for the ground station failsafeuint8_t rc_override_active : 1; // 14 // true if rc control are overwritten by ground stationuint8_t do_flip : 1; // 15 // Used to enable flip codeuint8_t takeoff_complete : 1; // 16uint8_t land_complete : 1; // 17uint8_t compass_status : 1; // 18uint8_t gps_status : 1; // 19};uint32_t value;} ap;static struct AP_System{uint8_t GPS_light : 1; // 0 // Solid indicates we have full 3D lock and can navigate, flash = readuint8_t arming_light : 1; // 1 // Solid indicates armed state, flashing is disarmed, double flashing is disarmed and failing pre-arm checksuint8_t new_radio_frame : 1; // 2 // Set true if we have new PWM data to act on from the Radiouint8_t CH7_flag : 1; // 3 // true if ch7 aux switch is highuint8_t CH8_flag : 1; // 4 // true if ch8 aux switch is highuint8_t usb_connected : 1; // 5 // true if APM is powered from USB connectionuint8_t yaw_stopped : 1; // 6 // Used to manage the Yaw hold capabilities } ap_system;////////////////////////////////////////////////////////////////////////////////// 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;////////////////////////////////////////////////////////////////////////////////// Motor Output////////////////////////////////////////////////////////////////////////////////#if FRAME_CONFIG == QUAD_FRAME#define MOTOR_CLASS AP_MotorsQuad#endif#if FRAME_CONFIG == TRI_FRAME#define MOTOR_CLASS AP_MotorsTri#endif#if FRAME_CONFIG == HEXA_FRAME#define MOTOR_CLASS AP_MotorsHexa#endif#if FRAME_CONFIG == Y6_FRAME#define MOTOR_CLASS AP_MotorsY6#endif#if FRAME_CONFIG == OCTA_FRAME#define MOTOR_CLASS AP_MotorsOcta#endif#if FRAME_CONFIG == OCTA_QUAD_FRAME#define MOTOR_CLASS AP_MotorsOctaQuad#endif#if FRAME_CONFIG == HELI_FRAME#define MOTOR_CLASS AP_MotorsHeli#endif#if FRAME_CONFIG == HELI_FRAME // helicopter constructor requires more arguments static 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.heli_servo_3, &g.heli_servo_4);#elif FRAME_CONFIG == TRI_FRAME // tri constructor requires additional rc_7 argument to allow tail servo reversingstatic MOTOR_CLASS motors(&g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, &g.rc_7);#elsestatic MOTOR_CLASS motors(&g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4);#endif////////////////////////////////////////////////////////////////////////////////// PIDs////////////////////////////////////////////////////////////////////////////////// This is a convienience accessor for the IMU roll rates. It's currently the raw IMU rates// and not the adjusted omega rates, but the name is stuckstatic Vector3f omega;// This is used to hold radio tuning values for in-flight CH6 tuningfloat tuning_value;// used to limit the rate that the pid controller output is logged so that it doesn't negatively affect performancestatic uint8_t pid_log_counter;////////////////////////////////////////////////////////////////////////////////// LED output////////////////////////////////////////////////////////////////////////////////// This is current status for the LED lights state machine// setting this value changes the output of the LEDsstatic uint8_t led_mode = NORMAL_LEDS;// Blinking indicates GPS statusstatic uint8_t copter_leds_GPS_blink;// Blinking indicates battery statusstatic uint8_t copter_leds_motor_blink;// Navigation confirmation blinksstatic int8_t copter_leds_nav_blink;////////////////////////////////////////////////////////////////////////////////// GPS variables////////////////////////////////////////////////////////////////////////////////// This is used to scale GPS values for EEPROM storage// 10^7 times Decimal GPS means 1 == 1cm// This approximation makes calculations integer and it's easy to readstatic const float t7 = 10000000.0;// We use atan2 and other trig techniques to calaculate angles// We need to scale the longitude up to make these calcs work// to account for decreasing distance between lines of longitude away from the equatorstatic float scaleLongUp = 1;// Sometimes we need to remove the scaling for distance calcsstatic float scaleLongDown = 1;////////////////////////////////////////////////////////////////////////////////// Location & Navigation////////////////////////////////////////////////////////////////////////////////// This is the angle from the copter to the next waypoint in centi-degreesstatic int32_t wp_bearing;// The original bearing to the next waypoint. used to point the nose of the copter at the next waypointstatic int32_t original_wp_bearing;// The location of home in relation to the copter in centi-degreesstatic int32_t home_bearing;// distance between plane and home in cmstatic int32_t home_distance;// distance between plane and next waypoint in cm.static uint32_t wp_distance;// navigation mode - options include NA V_NONE, NAV_LOITER, NA V_CIRCLE, NA V_WP static uint8_t nav_mode;// Register containing the index of the current navigation command in the mission scriptstatic int16_t command_nav_index;// Register containing the index of the previous navigation command in the mission script// Used to manage the execution of conditional commandsstatic uint8_t prev_nav_index;// Register containing the index of the current conditional command in the mission scriptstatic uint8_t command_cond_index;// Used to track the required WP navigation information// options include// NA V_ALTITUDE - have we reached the desired altitude?// NA V_LOCATION - have we reached the desired location?// NA V_DELAY - have we waited at the waypoint the desired time?static float lon_error, lat_error; // Used to report how many cm we are from the next waypoint or loiter target positionstatic int16_t control_roll;static int16_t control_pitch;static uint8_t rtl_state; // records state of rtl (initial climb, returning home, etc) static uint8_t land_state; // records state of land (flying to location, descending)////////////////////////////////////////////////////////////////////////////////// Orientation////////////////////////////////////////////////////////////////////////////////// Convienience accessors for commonly used trig functions. These values are generated// by the DCM through a few simple equations. They are used throughout the code where cos and sin// would normally be used.// The cos values are defaulted to 1 to get a decent initial value for a level statestatic float cos_roll_x = 1.0;static float cos_pitch_x = 1.0;static float cos_yaw = 1.0;static float sin_yaw;static float sin_roll;static float sin_pitch;////////////////////////////////////////////////////////////////////////////////// SIMPLE Mode////////////////////////////////////////////////////////////////////////////////// Used to track the orientation of the copter for Simple mode. This value is reset at each arming// or in SuperSimple mode when the copter leaves a 20m radius from home.static int32_t initial_simple_bearing;// Stores initial bearing when armed - initial simple bearing is modified in super simple mode so not suitablestatic int32_t initial_armed_bearing;////////////////////////////////////////////////////////////////////////////////// Rate contoller targets////////////////////////////////////////////////////////////////////////////////static uint8_t rate_targets_frame = EARTH_FRAME; // indicates whether rate targets provided in earth or body framestatic int32_t roll_rate_target_ef;static int32_t pitch_rate_target_ef;static int32_t yaw_rate_target_ef;static int32_t roll_rate_target_bf; // body frame roll rate targetstatic int32_t pitch_rate_target_bf; // body frame pitch rate targetstatic int32_t yaw_rate_target_bf; // body frame yaw rate target////////////////////////////////////////////////////////////////////////////////// Throttle variables////////////////////////////////////////////////////////////////////////////////static int16_t throttle_accel_target_ef; // earth frame throttle acceleration targetstatic bool throttle_accel_controller_active; // true when accel based throttle controller is active, false when higher level throttle controllers are providing throttle output directlystatic float throttle_avg; // g.throttle_cruise as a floatstatic int16_t desired_climb_rate; // pilot desired climb rate - for logging purposes onlystatic float target_alt_for_reporting; // target altitude in cm for reporting (logs and ground station)////////////////////////////////////////////////////////////////////////////////// ACRO Mode////////////////////////////////////////////////////////////////////////////////// Used to control Axis lockstatic int32_t roll_axis;static int32_t pitch_axis;// Filters#if FRAME_CONFIG == HELI_FRAMEstatic LowPassFilterFloat rate_roll_filter; // Rate Roll filterstatic LowPassFilterFloat rate_pitch_filter; // Rate Pitch filter// LowPassFilterFloat rate_yaw_filter; // Rate Yaw filter#endif // HELI_FRAME////////////////////////////////////////////////////////////////////////////////// Circle Mode / Loiter control////////////////////////////////////////////////////////////////////////////////Vector3f circle_center; // circle position expressed in cm from home location. x = lat, y = lon// angle from the circle center to the copter's desired location. Incremented at circle_rate / second static float circle_angle;// the total angle (in radians) travelledstatic float circle_angle_total;// deg : how many times to circle as specified by mission commandstatic uint8_t circle_desired_rotations;static float circle_angular_acceleration; // circle mode's angular accelerationstatic float circle_angular_velocity; // circle mode's angular velocitystatic float circle_angular_velocity_max; // circle mode's max angular velocity// How long we should stay in Loiter Mode for mission scripting (time in seconds)static uint16_t loiter_time_max;// How long have we been loitering - The start time in millisstatic uint32_t loiter_time;////////////////////////////////////////////////////////////////////////////////// CH7 and CH8 save waypoint control////////////////////////////////////////////////////////////////////////////////// This register tracks the current Mission Command index when writing// a mission using Ch7 or Ch8 aux switches in flightstatic int8_t aux_switch_wp_index;////////////////////////////////////////////////////////////////////////////////// Battery Sensors////////////////////////////////////////////////////////////////////////////////// Battery Voltage of battery, initialized above threshold for filterstatic float battery_voltage1 = LOW_VOLTAGE * 1.05f;// refers to the instant amp draw – based on an Attopilot Current sensorstatic float current_amps1;// refers to the total amps drawn – based on an Attopilot Current sensorstatic float current_total1;////////////////////////////////////////////////////////////////////////////////// Altitude////////////////////////////////////////////////////////////////////////////////// The (throttle) controller desired altitude in cmstatic float controller_desired_alt;// The cm we are off in altitude from next_WP.alt – Positive value means we are below the WP static int32_t altitude_error;// The cm/s we are moving up or down based on filtered data - Positive = UPstatic int16_t climb_rate;// The altitude as reported by Sonar in cm – Values are 20 to 700 generally.static int16_t sonar_alt;static uint8_t sonar_alt_health; // true if we can trust the altitude from the sonar// The altitude as reported by Baro in cm – Values can be quite highstatic int32_t baro_alt;static int16_t saved_toy_throttle;////////////////////////////////////////////////////////////////////////////////// flight modes////////////////////////////////////////////////////////////////////////////////// Flight modes are combinations of Roll/Pitch, Yaw and Throttle control modes// Each Flight mode is a unique combination of these modes//// The current desired control scheme for Yawstatic uint8_t yaw_mode;// The current desired control scheme for roll and pitch / navigationstatic uint8_t roll_pitch_mode;// The current desired control scheme for altitude holdstatic uint8_t throttle_mode;////////////////////////////////////////////////////////////////////////////////// flight specific////////////////////////////////////////////////////////////////////////////////// An additional throttle added to keep the copter at the same altitude when banking static int16_t angle_boost;// counter to verify landingsstatic uint16_t land_detector;////////////////////////////////////////////////////////////////////////////////// 3D Location vectors////////////////////////////////////////////////////////////////////////////////// home location is stored when we have a good GPS lock and arm the copter// Can be reset each the copter is re-armedstatic struct Location home;// Current location of the copterstatic struct Location current_loc;// Holds the current loaded command from the EEPROM for navigationstatic struct Location command_nav_queue;// Holds the current loaded command from the EEPROM for conditional scripts static struct Location command_cond_queue;////////////////////////////////////////////////////////////////////////////////// Navigation Roll/Pitch functions////////////////////////////////////////////////////////////////////////////////// all angles are deg * 100 : target yaw angle// The Commanded ROll from the autopilot.static int32_t nav_roll;// The Commanded pitch from the autopilot. negative Pitch means go forward.。
apm代码解析
apm代码解析
APM（Application Performance Management）代码解析是指使用APM工具对应用程序代码进行分析和优化的一种方法。
APM工具可以捕获和分析应用程序的性能数据，并提供有关代码性能的详细信息。
通过代码解析，开发人员可以识别代码中的瓶颈和性能问题，并采取措施进行优化。
在APM代码解析中，工具通常会收集以下数据：
1. 请求数据：包括请求的时间、响应时间、请求的参数和头信息等。
2. 内存数据：包括内存使用情况、堆栈跟踪和线程状态等。
3. 数据库数据：包括数据库查询、事务和锁等。
4. 网络数据：包括网络请求、响应和延迟等。
APM工具会根据这些数据生成报告和图表，以帮助开发人员识别代码中的性能问题。
报告和图表通常包括以下内容：
1. 性能指标：包括响应时间、吞吐量、错误率等。
2. 代码性能：包括代码中的热点方法、慢方法、阻塞方法等。
3. 内存使用：包括内存泄漏、内存不足等问题。
4. 数据库查询：包括慢查询、重复查询等问题。
5. 网络请求：包括网络延迟、高延迟请求等问题。
通过分析这些报告和图表，开发人员可以识别代码中的性能问题，并采取措施进行优化。
例如，开发人员可以对慢方法进行优化、减少内存使用、优化数据库查询和减少网络延迟等。
总之，APM代码解析是一种使用APM工具对应用程序代码进行分析和优化的一种方法。
通过代码解析，开发人员可以识别代码中的性能问题，并采取措施进行优化，从而提高应用程序的性能和用户体验。
详细的APM飞控调试资料
调整ArduCopter 参数如果你使用的机身不是官方ArduCopter 套件,你可能需要改变一些PID设置（PID 是比例-积分- 微分的简称，是一个标准的控制方法。
更多的资料在这里）。
在此页底部的有一个PID的全面的指导.你可以在任务规划器的配置选项卡中以交互方式调整PID：基本性能故障排除•我的多旋翼在稳定模式下缓慢震荡（大幅运动）: 降低 STABILIZE_ROLL_P，STABILIZE_PITCH_P.•我的多旋翼在稳定模式下*＊*震荡(小幅运动）: 降低 RATE_ROLL_P, RATE_PITCH_P。
•我的飞机过于迟钝：降低 RATE_ROLL_P，RATE_PITCH_P，和/或增加 STABILIZE_ROLL_P, STABILIZE_PITCH_P.•我调整了 Rate_P，还是不行：也许你的 STABILIZE_P gain 增益过高。
降低一点（见上文）,并再次尝试调整 RATE_P.•我的飞机在起飞时向左或向右旋转15°:你的电机不直或着电调没有校准。
扭转电机，直到他们都直了。
运行ESC校准程序。
•激烈飞行后我的飞机偏向一方 10 - 30°：如该文所述，焊接 IMU 的滤波器U。
你可以在 system.pde 里调整漂移校正。
如果需要，大概调高0。
5.此外，降落30秒，然后继续飞行。
•我的飞机无法在空中保持完全静止：确保在飞机的重心在正中心。
然后在水平面上运行水平命令（保持关闭状态15秒，调用该功能）.你也可以在无风的环境（重要）使用自动微调模式飞行。
任何风将导致四轴旋转180度后你的修改产生相反的作用。
你可以使用遥控俯仰和横滚微调，但记得在用配置工具设置遥控时,要把它们放回中心.我不喜欢使用发射微调，但永远不要使用偏航微调.（四轴也很容易受到紊流的影响。
他们将需要不断的修正，除非你安装一个光流传感器。
某天……）•我的飞机飞行很好，但后来在悬停时一条电机臂奇怪地下降了:你的电机坏了。
APM飞控介绍范文
APM飞控介绍范文APM（ArduPilot Mega）飞控是一款开源的无人机飞行控制器，使用Arduino Mega 2560开发板和ATmega2560微控制器进行控制。
它可以支持多种飞行器类型，包括多旋翼、固定翼、直升机、车辆和船只等，且适用于初学者和专业人士。
APM飞控的功能非常强大，具备多种传感器与功能模块的接口，包括陀螺仪、加速度计、罗盘、GPS、气压计、导航模块、通信模块等。
这些传感器和模块提供了飞行姿态稳定性、位置定位、导航、高度控制、避障等功能。
APM飞控使用可视化的图形用户界面（Ground Control Station，简称GCS）来进行配置和控制。
用户可以通过电脑、手机或平板等设备与APM飞控进行通信，实时获取飞行数据，在线调整参数和模式，进行飞行计划等。
1.多种飞行器类型支持：APM飞控可以支持各种飞行器类型的控制，包括四旋翼、六旋翼、八旋翼、固定翼、直升机等。
通过选择不同的飞行器类型，用户可以针对不同的应用场景进行配置和飞行。
2.多种飞行模式：APM飞控支持多种飞行模式，包括手动模式、稳定模式、姿态模式、定高模式、定点模式、跟随模式、航点模式等。
用户可以根据需求选择不同的飞行模式，以实现自由飞行、稳定飞行、自动飞行等功能。
3.导航和定位功能：APM飞控可以通过GPS进行导航和定位，实现自动驾驶功能。
用户可以设置航点和航线，飞行器能够自动按照设定的航线进行飞行，同时实时在GCS上显示当前位置和飞行状态。
4.传感器和稳定性：APM飞控配备了陀螺仪、加速度计和罗盘等传感器，能够实时获取飞行器的姿态信息。
通过PID控制算法和传感器反馈，可以实现飞行器的姿态稳定和控制。
5.遥控器和数据链路：APM飞控支持与遥控器和数据链路进行通信和控制。
用户可以通过遥控器操控飞行器的飞行，实现手动控制、姿态控制等功能。
同时，用户还可以通过数据链路将APM飞控与地面站进行通信，实时获取飞行数据和调整参数。
APM飞控源码讲解
navigate( )，正是在这个函数中，执行了导航航向角nav_bearing的计算。
navigate( )中有：
= get_bearing(¤t_loc, &next_WP);
current_loc和next_WP是结构体，里面存储这一个位置点的经度、纬度、
必须在其基础上进行适当修改。以下以使用空速计情况和不使
APM飞控系统进行阐述。
1），使用空速计情况
update_current_flight_mode( )调用
调用nav_pitch = -g.pidNavPitchAirspeed.get_pid(airspeed_error, dTnav)。
、(g.channel_throttle.servo_out * g.kff_throttle_to_pitch)和
。其作用如下：
作用： 加入这个是因为飞机滚转时时会
所以提前加入了掉高度的预判fabs(dcm.roll_sensor * g.kff_pitch_compensation)。
g.kff_pitch_compensation默认值是0.3。
+= constrain(crosstrack_error * g.crosstrack_gain,
g.crosstrack_entry_angle.get())，这样其实就把目标航向角
nav_bearing 中。
飞控已经知道该怎么让飞机飞行了，现在就要解决飞控如何具体控制飞机的问
1）油门的控制
4）方向舵的控制
stabilize( )调用calc_nav_yaw(speed_scaler)
APM代码学习笔记1
APM代码学习笔记1libraries⽬录传感器AP_InertialSensor 惯性导航传感器就是陀螺仪加速计AP_Baro ⽓压计居然⽀持BMP085 在我印象中APM⼀直⽤⾼端的MS5611AP_Compass 指南针AP_GPS GPS定位还有些飞⾏姿态的AP_ARHS 姿态解算输出Roll Yaw PitchAP_AttitudeControl 姿态控制 APM飞这么稳就靠它了硬件抽象层AP_HAL 头⽂件AP_HAL_AVR APM2.X时代的板⼦ ATMega2560处理器AP_HAL_FLYMAPLE 好⼏年前中国⼀个团队出的基于Maple(STM32版Arduino)飞控 从硬件规格上STM32RET6 ITG3205 ADXL345 HMC5883 BMP085 按理说挺有发展前景但不知怎么没了下⽂AP_HAL_Linux 基于嵌⼊式Linux的飞控有树莓派加扩展板 uavio+ 国内的raspilot BeagleBoard加扩展板Erle Brain 2 也有商品飞⾏器Parrot Bebop Drone 应该是⽬前最有发展前景的平台。
AP_HAL_PX4 Pixhawk ⽬前主推的平台⽤料⾜价格贵不亚于上⾯Linux的那种性价⽐不⾼AP_HAL_SITL 模拟器不⽤买硬件直接在电脑上模拟了解⽰例代码定义hal变量作为引⽤代码粗糙缺乏注释setup() loop()函数和arduino⼀样hal引⽤每⼀个⽤到HAL层的⽂件都需要hal变量它获得AP_HAL::HAL对象，提供所有硬件特性的调⽤，包括打印消息到控制台，睡眠，I2C和SPI总线实际的hal实现都在AP_HAL_XXX库中常⽤hal⽅法有(类似arduino)hal.console->printf() hal.console->printf_P() 打印消息到控制台(_P在avr上能节约内存)hal.scheduler->millis() hal.scheduler->micros() 启动时间hal.scheduler->delay() hal.scheduler->delay_microseconds() 延迟指定时间hal.gpio->pinMode() hal.gpio->read() hal.gpio->write() 设置读写gpiohal.i2c I2C访问hal.spi SPI访问AP_HAL_MAIN 宏做⼀些HAL层的初始化通常不⽤关⼼它的具体实现Hello World⽐上⾯的还简单建⽴⽬录\libraries\AP_HelloWorld\examples\HelloWorld_test 建⽴三个⽂件HelloWorld_test.cpp#include <AP_HAL/AP_HAL.h>const AP_HAL::HAL& hal = AP_HAL::get_HAL();void setup(){hal.console->println("Hello World");}void loop(){hal.scheduler->delay(10);//必须延时不然上⾯的println都不会执⾏}AP_HAL_MAIN();make.incLIBRARIES += AP_HelloWorld只编译当前库Makefileinclude ../../../../mk/apm.mk可以从GPS_AUTO_test复制开始编译make linux -j4运⾏sudo ./HelloWorld_test.elf。
APM2.52飞控板测试文档(修改)
APM2.52飞控板功能测试文档图1-1 APM2.52飞控所属部门：技术部文档作者：jack文档类别：测试文档评审日期：2014-08-07产品系列：APM飞控发布日期：2014-08-10公开部门：技术部生产部文档状态：发布1 测试概要测试APM2.52烧写：①Atmega2560的bootloader烧写②Atmega32U2的bootloader烧写③Atmega32U2的PPM解码文件烧写④连接地面站烧写四轴固件⑤测试飞控各个传感器工作是否正常2 测试环境测试硬件平台：APM2.52飞控板，usbasp编程器一根，usb数据线一根（扁口）测试软件平台：progisp1.72，MissionPlanner，JRE - Flip Installer ,信科中对应项目中下载如下图：3 测试代码无测试代码4 测试用例硬件连接：APM2.52飞控板与usbasp编程器，1脚位置对应烧写器一脚位置，如图所示如上图：CBA灯左上角的跳线帽可加可不加，加上的作用为电调给飞控供电。
测试完成时请带上跳线帽，方便入库。
一：atmega2560 bootload烧写1. 1、Atmega2560的SPI接口在APM板靠近USB接口位置，为双排6PIN排针，连接好下载线后打开progisp，select chip选择Atmega2560，然后RD一下ID识别字是否对应，没有错误的话继续下一步2. 点击预写熔丝或者编程熔丝右边的数字框进行Atmega2560的熔丝位配置3.Atmega2560的熔丝位如下配置：低FF，高D8，扩展FD，配置好后点击写入执行熔丝位的写入，写入完成后可以点击读出熔丝位验证熔丝位是否已经写入，写入成功即可关闭熔丝位配置界面4.配置好熔丝位后点击“调入flash”载入Atmega2560的bootloader的hex文件，在主界面上勾选芯片擦除、编程flash、校验flash，然后点击自动开始写入bootloader程序，2560写入时间较长，需耐心等待5、烧写成功后会出现校验错误，这是因为progisp对于2560的bootloader校验功能不是很完善，校验不了，所以才会出现这样的校验错误。
APM飞控姿态角控制系统
On every update (i.e. 400hz on Pixhawk, 100hz on APM2.x) the following happens: 1、the top level flight-mode.cpp’s “update_flight_mode()” function is called. This function checks the vehicle’s flight mode (ie. “control_mode” variable) and then calls the appropriate <flight mode>_run() function (i.e. stabilize_run for stabilize mode, rtl_run for RTL mode, etc). The <flight mode>_run() function can be found in the appropriately named control_<flight mode>.cpp file (i.e.control_stabilize.cpp, control_rtl.cpp, etc).2、the <flight mode>_run function is responsible for converting the user’s input (found in g.rc_1.control_in, g.rc_2.control_in, etc) into a lean angle, rotation rate, climb rate, etc that is appropriate for this flight mode. For example AltHold converts the user’s roll and pitch input into lean angles (in degrees), the yaw input is converted into a rotation rate (in degrees per second) and the throttle input is converted to a climb rate (in cm/s).3、the last thing the <flight mode>_run function must do is pass these desired angles, rates etc into Attitude Control and/or Position Control libraries (these are both held in the AC_AttitudeControlfolder).4、The AC_AttitudeControl library provides 5 possible ways to control the attitude of the vehicle, the most common 3 are described below.（1）、angle_ef_roll_pitch_rate_ef_yaw() : this accepts an “earth frame” angle for roll and pitch, and an“earth frame” rate for yaw. For example providing this function roll = -1000, pitch = -1500, yaw = 500 means lean the vehicle left to 10degrees, pitch forward to 15degrees and rotate right at 5deg/second（2）、angle_ef_roll_pitch_yaw() : this accepts “earth frame” angles for roll, pitch and yaw. similar to above except providing yaw of 500 means rotate the vehicle to 5 degrees east of north.（3）、rate_bf_roll_pitch_yaw() : this accepts a “body frame” rate (in degrees/sec) for roll pitch and yaw. For example providing this function roll = -1000, pitch = -1500, yaw = 500 would lead to the vehicle rolling left at 10deg/sec, pitching forward at 15deg/sec and rotating about the vehicle’s z axis at 5 deg/sec.5、可以看到，是在3中调用了4中的任意一个函数，这样就可以调用rate_controller_run()了After any calls to these functions are made the AC_AttitudeControl::rate_controller_run() is called. This converts the output from the methods listed above into roll, pitch and yaw inputs which are sent to the AP_Motors library via it’s set_roll, set_pitch, set_yaw and set_throttle methods. .6、The AC_PosControl library allows 3D position control of the vehicle. Normally only the simpler Z-axis(i.e. altitude control) methods are used because more complicated 3D position flight modes (i.e. Loiter) make use of the AC_WPNav library. In any case, some commonly used methods of this library include:set_alt_target_from_climb_rate() : this accepts a climb rate in cm/s and updates an absolute altitude targetset_pos_target() : this accepts a 3D position vector which is an offset from home in cmIf any methods in AC_PosControl are called then the flight mode code must also call theAC_PosControl::update_z_controller() method.This will run the z-axis position control PID loops and send low-level throttle level to the AP_Motors library. If any xy-axis methods are called thenAC_PosControl::update_xy_controller() must be called.7、The AP_Motors library holds the “motor mixing” code. This code is responsible for converting the roll, pitch, yaw and throttle values received from the AC_AttitudeControl and AC_PosControl libraries into absolute motor outputs(i.e. PWM values). So the higher level libs would make use of these functions:set_roll(), set_pitch(), set_yaw() : accepts roll, pitch and yaw values in the range of -4500 ~ 4500. These are not desired angles or even rates but rather just a value. For example set_roll(-4500) would mean roll left as fast as possible.set_throttle() : accepts an absolute throttle value in the range of 0 ~ 1000. 0 = motors off, 1000 = full throttle.There are different classes for each frame type (quad, Y6, traditional helicopter) but in each there is an “output_armed” function which is responsible for implementing the conversion of these roll, pitch, yaw and throttle values into pwm outputs. This conversion often includes implementing a “stability patch” which handles prioritising one axis of control over another when the input requests are outside the physical limits of the frame (i.e. max throttle and max roll is not possible with a quad because some motors must be less than others to cause a roll). At the bottom of the “output_armed” function there is a call to the hal.rcout->write() which passes the desired pwm values to the AP_HAL layer.。
APM飞控程序解读
/// -*- 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 modifyit under the terms of the GNU General Public License as published bythe 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 ofMERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See theGNU General Public License for more details.You should have received a copy of the GNU General Public Licensealong with this program. If not, see </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* 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 <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_PX4.h>#include <AP_HAL_FLYMAPLE.h>#include <AP_HAL_Linux.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/Matrix 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//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_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 drivers static 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* 1 Ailerons (rudder if no ailerons)* 2 Elevator* 3 Throttle* 4 Rudder (if we have ailerons)* 5 Mode - 3 position switch* 6 User assignable* 7 trainer switch - sets throttle nominal (toggle switch), sets accels to Level (hold > 1 second)* 8 TBD* 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; // 0uint8_t simple_mode : 2; // 1,2 // This is the state of simple mode : 0 = disabled ; 1 = SIMPLE ;2 = SUPERSIMPLEuint8_t pre_arm_rc_check : 1; // 3 // true if rc input pre-arm checks have been completed successfullyuint8_t pre_arm_check : 1; // 4 // true if all pre-arm checks (rc, accel calibration, gps lock) have been performeduint8_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 starteduint8_t do_flip : 1; // 7 // Used to enable flip codeuint8_t takeoff_complete : 1; // 8uint8_t land_complete : 1; // 9 // true if we have detected a landinguint8_t new_radio_frame : 1; // 10 // Set true if we have new PWM data to act on from the Radiouint8_t CH7_flag : 2; // 11,12 // ch7 aux switch : 0 is low or false, 1 is center or true, 2 is highuint8_t CH8_flag : 2; // 13,14 // ch8 aux switch : 0 is low or false, 1 is center or true, 2 is highuint8_t usb_connected : 1; // 15 // true if APM is powered from USB connectionuint8_t yaw_stopped : 1; // 16 // Used to manage the Yaw hold capabilitiesuint8_t disable_stab_rate_limit : 1; // 17 // disables limits rate request from the stability controlleruint8_t rc_receiver_present : 1; // 18 // true if we have an rc receiver present (i.e. if we've 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 failsafeuint8_t battery : 1; // 2 // A status flag for the battery failsafeuint8_t gps : 1; // 3 // A status flag for the gps failsafeuint8_t gcs : 1; // 4 // A status flag for the ground station failsafeint8_t radio_counter; // number of iterations with throttle below throttle_fs_valueuint32_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.heli_servo_3, &g.heli_servo_4);#elif FRAME_CONFIG == TRI_FRAME // tri constructor requires additional rc_7 argument to allow tail servo reversingstatic MOTOR_CLASS motors(&g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, &g.rc_7);#elsestatic MOTOR_CLASS motors(&g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4);#endif////////////////////////////////////////////////////////////////////////////////// PIDs////////////////////////////////////////////////////////////////////////////////// This is a convienience accessor for the IMU roll rates. It's currently the raw IMU rates// and not the adjusted omega rates, but the name is stuckstatic Vector3f omega;// This is used to hold radio tuning values for in-flight CH6 tuningfloat tuning_value;// used to limit the rate that the pid controller output is logged so that it doesn't negatively affect performancestatic uint8_t pid_log_counter;////////////////////////////////////////////////////////////////////////////////// LED output////////////////////////////////////////////////////////////////////////////////// Blinking indicates GPS statusstatic uint8_t copter_leds_GPS_blink;// Blinking indicates battery statusstatic uint8_t copter_leds_motor_blink;// Navigation confirmation blinksstatic int8_t copter_leds_nav_blink;// GPS variables////////////////////////////////////////////////////////////////////////////////// This is used to scale GPS values for EEPROM storage// 10^7 times Decimal GPS means 1 == 1cm// This approximation makes calculations integer and it's easy to readstatic const float t7 = 10000000.0;// We use atan2 and other trig techniques to calaculate angles// We need to scale the longitude up to make these calcs work// to account for decreasing distance between lines of longitude away from the equatorstatic float scaleLongUp = 1;// Sometimes we need to remove the scaling for distance calcsstatic float scaleLongDown = 1;////////////////////////////////////////////////////////////////////////////////// Location & Navigation////////////////////////////////////////////////////////////////////////////////// This is the angle from the copter to the next waypoint in centi-degreesstatic int32_t wp_bearing;// The original bearing to the next waypoint. used to point the nose of the copter at the next waypoint static int32_t original_wp_bearing;// The location of home in relation to the copter in centi-degreesstatic int32_t home_bearing;// distance between plane and home in cmstatic int32_t home_distance;// distance between plane and next waypoint in cm.static uint32_t wp_distance;// navigation mode - options include NAV_NONE, NAV_LOITER, NAV_CIRCLE, NAV_WPstatic uint8_t nav_mode;// Register containing the index of the current navigation command in the mission scriptstatic int16_t command_nav_index;// Register containing the index of the previous navigation command in the mission script// Used to manage the execution of conditional commandsstatic uint8_t prev_nav_index;// Register containing the index of the current conditional command in the mission scriptstatic uint8_t command_cond_index;// Used to track the required WP navigation information// options include// NAV_ALTITUDE - have we reached the desired altitude?// NAV_LOCATION - have we reached the desired location?// NAV_DELAY - have we waited at the waypoint the desired time?static float lon_error, lat_error; // Used to report how many cm we are from the next waypoint or loiter target positionstatic int16_t control_roll;static int16_t control_pitch;static uint8_t rtl_state; // records state of rtl (initial climb, returning home, etc) static uint8_t land_state; // records state of land (flying to location, descending)////////////////////////////////////////////////////////////////////////////////// Orientation////////////////////////////////////////////////////////////////////////////////// Convienience accessors for commonly used trig functions. These values are generated// by the DCM through a few simple equations. They are used throughout the code where cos and sin// would normally be used.// The cos values are defaulted to 1 to get a decent initial value for a level statestatic float cos_roll_x = 1.0;static float cos_pitch_x = 1.0;static float cos_yaw = 1.0;static float sin_yaw;static float sin_roll;static float sin_pitch;// SIMPLE Mode////////////////////////////////////////////////////////////////////////////////// Used to track the orientation of the copter for Simple mode. This value is reset at each arming // or in SuperSimple mode when the copter leaves a 20m radius from home.static float simple_cos_yaw = 1.0;static float simple_sin_yaw;static int32_t super_simple_last_bearing;static float super_simple_cos_yaw = 1.0;static float super_simple_sin_yaw;// Stores initial bearing when armed - initial simple bearing is modified in super simple mode so not suitable static int32_t initial_armed_bearing;////////////////////////////////////////////////////////////////////////////////// Rate contoller targets////////////////////////////////////////////////////////////////////////////////static uint8_t rate_targets_frame = EARTH_FRAME; // indicates whether rate targets provided in earth or body framestatic int32_t roll_rate_target_ef;static int32_t pitch_rate_target_ef;static int32_t yaw_rate_target_ef;static int32_t roll_rate_target_bf; // body frame roll rate targetstatic int32_t pitch_rate_target_bf; // body frame pitch rate targetstatic int32_t yaw_rate_target_bf; // body frame yaw rate target////////////////////////////////////////////////////////////////////////////////// Throttle variables////////////////////////////////////////////////////////////////////////////////static int16_t throttle_accel_target_ef; // earth frame throttle acceleration targetstatic bool throttle_accel_controller_active; // true when accel based throttle controller is active, false when higher level throttle controllers are providing throttle output directlystatic float throttle_avg; // g.throttle_cruise as a floatstatic int16_t desired_climb_rate; // pilot desired climb rate - for logging purposes only static float target_alt_for_reporting; // target altitude in cm for reporting (logs and ground station)////////////////////////////////////////////////////////////////////////////////// ACRO Mode////////////////////////////////////////////////////////////////////////////////// Used to control Axis lockstatic int32_t acro_roll; // desired roll angle while sport modestatic int32_t acro_roll_rate; // desired roll rate while in acro modestatic int32_t acro_pitch; // desired pitch angle while sport modestatic int32_t acro_pitch_rate; // desired pitch rate while acro modestatic int32_t acro_yaw_rate; // desired yaw rate while acro modestatic float acro_level_mix; // scales back roll, pitch and yaw inversely proportional to input from pilot// Filters#if FRAME_CONFIG == HELI_FRAME//static LowPassFilterFloat rate_roll_filter; // Rate Roll filter//static LowPassFilterFloat rate_pitch_filter; // Rate Pitch filter#endif // HELI_FRAME////////////////////////////////////////////////////////////////////////////////// Circle Mode / Loiter control////////////////////////////////////////////////////////////////////////////////Vector3f circle_center; // circle position expressed in cm from home location. x = lat, y = lon // angle from the circle center to the copter's desired location. Incremented at circle_rate / secondstatic float circle_angle;// the total angle (in radians) travelledstatic float circle_angle_total;// deg : how many times to circle as specified by mission commandstatic uint8_t circle_desired_rotations;static float circle_angular_acceleration; // circle mode's angular accelerationstatic float circle_angular_velocity; // circle mode's angular velocitystatic float circle_angular_velocity_max; // circle mode's max angular velocity// How long we should stay in Loiter Mode for mission scripting (time in seconds)static uint16_t loiter_time_max;// How long have we been loitering - The start time in millisstatic uint32_t loiter_time;////////////////////////////////////////////////////////////////////////////////// CH7 and CH8 save waypoint control////////////////////////////////////////////////////////////////////////////////// This register tracks the current Mission Command index when writing// a mission using Ch7 or Ch8 aux switches in flightstatic int8_t aux_switch_wp_index;////////////////////////////////////////////////////////////////////////////////// Battery Sensors////////////////////////////////////////////////////////////////////////////////static AP_BattMonitor battery;////////////////////////////////////////////////////////////////////////////////// Altitude////////////////////////////////////////////////////////////////////////////////// The (throttle) controller desired altitude in cmstatic float controller_desired_alt;// The cm we are off in altitude from next_WP.alt – Positive value means we are below the WP static int32_t altitude_error;// The cm/s we are moving up or down based on filtered data - Positive = UPstatic int16_t climb_rate;// The altitude as reported by Sonar in cm – Values are 20 to 700 generally.static int16_t sonar_alt;static uint8_t sonar_alt_health; // true if we can trust the altitude from the sonarstatic float target_sonar_alt; // desired altitude in cm above the ground// The altitude as reported by Baro in cm – Values can be quite highstatic int32_t baro_alt;static int16_t saved_toy_throttle;////////////////////////////////////////////////////////////////////////////////// flight modes////////////////////////////////////////////////////////////////////////////////// Flight modes are combinations of Roll/Pitch, Yaw and Throttle control modes// Each Flight mode is a unique combination of these modes//// The current desired control scheme for Yawstatic uint8_t yaw_mode;// The current desired control scheme for roll and pitch / navigationstatic uint8_t roll_pitch_mode;// The current desired control scheme for altitude holdstatic uint8_t throttle_mode;////////////////////////////////////////////////////////////////////////////////。
APM飞控程序解读
/// -*- 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 modifyit under the terms of the GNU General Public License as published bythe 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 ofMERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See theGNU General Public License for more details.You should have received a copy of the GNU General Public Licensealong with this program. If not, see </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* 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 <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_PX4.h>#include <AP_HAL_FLYMAPLE.h>#include <AP_HAL_Linux.h>#include <AP_HAL_Empty.h>// Application dependencies#include <GCS_MAVLink.h> // MAVLink GCS definitions#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/Matrix 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// AP_HAL to Arduino compatibility layer#include "compat.h"// Configuration#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_PX4static DataFlash_File DataFlash("/fs/microsd/APM/logs");#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUXstatic DataFlash_File DataFlash("logs");#elsestatic 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 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 drivers static 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* 1 Ailerons (rudder if no ailerons)* 2 Elevator* 3 Throttle* 4 Rudder (if we have ailerons)* 5 Mode - 3 position switch* 6 User assignable* 7 trainer switch - sets throttle nominal (toggle switch), sets accels to Level (hold >1 second)* 8 TBD* 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; // 0uint8_t simple_mode : 2; // 1,2 // This is the state of simple mode : 0 = disabled ; 1 = SIMPLE ;2 = SUPERSIMPLEuint8_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 performeduint8_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 starteduint8_t do_flip : 1; // 7 // Used to enable flip codeuint8_t takeoff_complete : 1; // 8uint8_t land_complete : 1; // 9 // true if we have detected a landinguint8_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 highuint8_t CH8_flag : 2; // 13,14 // ch8 aux switch : 0 is low or false, 1 is center or true, 2 is highuint8_t usb_connected : 1; // 15 // true if APM is powered from USB connectionuint8_t yaw_stopped : 1; // 16 // Used to manage the Yaw hold capabilitiesuint8_t disable_stab_rate_limit : 1; // 17 // disables limits rate request from the stability controlleruint8_t rc_receiver_present : 1; // 18 // true if we have an rc receiver present (i.e. if we've 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 stationuint8_t radio : 1; // 1 // A status flag for the radio failsafeuint8_t battery : 1; // 2 // A status flag for the battery failsafeuint8_t gps : 1; // 3 // A status flag for the gps failsafeuint8_t gcs : 1; // 4 // A status flag for the ground station failsafeint8_t radio_counter; // number of iterations with throttle below throttle_fs_valueuint32_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.heli_servo_3, &g.heli_servo_4);#elif FRAME_CONFIG == TRI_FRAME // tri constructor requires additional rc_7 argument to allow tail servo reversing static MOTOR_CLASS motors(&g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, &g.rc_7);#elsestatic MOTOR_CLASS motors(&g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4);#endif////////////////////////////////////////////////////////////////////////////////// PIDs////////////////////////////////////////////////////////////////////////////////// This is a convienience accessor for the IMU roll rates. It's currently the raw IMU rates// and not the adjusted omega rates, but the name is stuckstatic Vector3f omega;// This is used to hold radio tuning values for in-flight CH6 tuningfloat tuning_value;// used to limit the rate that the pid controller output is logged so that it doesn't negatively affect performance static uint8_t pid_log_counter;////////////////////////////////////////////////////////////////////////////////// LED output////////////////////////////////////////////////////////////////////////////////// Blinking indicates GPS statusstatic uint8_t copter_leds_GPS_blink;// Blinking indicates battery statusstatic uint8_t copter_leds_motor_blink;// Navigation confirmation blinksstatic int8_t copter_leds_nav_blink;////////////////////////////////////////////////////////////////////////////////// GPS variables////////////////////////////////////////////////////////////////////////////////// This is used to scale GPS values for EEPROM storage// 10^7 times Decimal GPS means 1 == 1cm// This approximation makes calculations integer and it's easy to readstatic const float t7 = 10000000.0;// We use atan2 and other trig techniques to calaculate angles// We need to scale the longitude up to make these calcs work// to account for decreasing distance between lines of longitude away from the equatorstatic float scaleLongUp = 1;// Sometimes we need to remove the scaling for distance calcsstatic float scaleLongDown = 1;////////////////////////////////////////////////////////////////////////////////// Location & Navigation////////////////////////////////////////////////////////////////////////////////// This is the angle from the copter to the next waypoint in centi-degreesstatic int32_t wp_bearing;// The original bearing to the next waypoint. used to point the nose of the copter at the next waypoint static int32_t original_wp_bearing;// The location of home in relation to the copter in centi-degreesstatic int32_t home_bearing;// distance between plane and home in cmstatic int32_t home_distance;// distance between plane and next waypoint in cm.static uint32_t wp_distance;// navigation mode - options include NAV_NONE, NAV_LOITER, NAV_CIRCLE, NAV_WPstatic uint8_t nav_mode;// Register containing the index of the current navigation command in the mission scriptstatic int16_t command_nav_index;// Register containing the index of the previous navigation command in the mission script// Used to manage the execution of conditional commandsstatic uint8_t prev_nav_index;// Register containing the index of the current conditional command in the mission scriptstatic uint8_t command_cond_index;// Used to track the required WP navigation information// options include// NAV_ALTITUDE - have we reached the desired altitude?// NAV_LOCATION - have we reached the desired location?// NAV_DELAY - have we waited at the waypoint the desired time?static float lon_error, lat_error; // Used to report how many cm we are from the next waypoint or loiter target positionstatic int16_t control_roll;static int16_t control_pitch;static uint8_t rtl_state; // records state of rtl (initial climb, returning home, etc)static uint8_t land_state; // records state of land (flying to location, descending)////////////////////////////////////////////////////////////////////////////////// Orientation////////////////////////////////////////////////////////////////////////////////// Convienience accessors for commonly used trig functions. These values are generated// by the DCM through a few simple equations. They are used throughout the code where cos and sin// would normally be used.// The cos values are defaulted to 1 to get a decent initial value for a level statestatic float cos_roll_x = 1.0;static float cos_pitch_x = 1.0;static float cos_yaw = 1.0;static float sin_yaw;static float sin_roll;static float sin_pitch;////////////////////////////////////////////////////////////////////////////////// SIMPLE Mode////////////////////////////////////////////////////////////////////////////////// Used to track the orientation of the copter for Simple mode. This value is reset at each arming// or in SuperSimple mode when the copter leaves a 20m radius from home.static float simple_cos_yaw = 1.0;static float simple_sin_yaw;static int32_t super_simple_last_bearing;static float super_simple_cos_yaw = 1.0;static float super_simple_sin_yaw;// Stores initial bearing when armed - initial simple bearing is modified in super simple mode so not suitable static int32_t initial_armed_bearing;////////////////////////////////////////////////////////////////////////////////// Rate contoller targets////////////////////////////////////////////////////////////////////////////////static uint8_t rate_targets_frame = EARTH_FRAME; // indicates whether rate targets provided in earth or body framestatic int32_t roll_rate_target_ef;static int32_t pitch_rate_target_ef;static int32_t yaw_rate_target_ef;static int32_t roll_rate_target_bf; // body frame roll rate targetstatic int32_t pitch_rate_target_bf; // body frame pitch rate targetstatic int32_t yaw_rate_target_bf; // body frame yaw rate target////////////////////////////////////////////////////////////////////////////////// Throttle variables////////////////////////////////////////////////////////////////////////////////static int16_t throttle_accel_target_ef; // earth frame throttle acceleration targetstatic bool throttle_accel_controller_active; // true when accel based throttle controller is active, false when higher level throttle controllers are providing throttle output directlystatic float throttle_avg; // g.throttle_cruise as a floatstatic int16_t desired_climb_rate; // pilot desired climb rate - for logging purposes onlystatic float target_alt_for_reporting; // target altitude in cm for reporting (logs and ground station)////////////////////////////////////////////////////////////////////////////////// ACRO Mode////////////////////////////////////////////////////////////////////////////////// Used to control Axis lockstatic int32_t acro_roll; // desired roll angle while sport modestatic int32_t acro_roll_rate; // desired roll rate while in acro modestatic int32_t acro_pitch; // desired pitch angle while sport modestatic int32_t acro_pitch_rate; // desired pitch rate while acro modestatic int32_t acro_yaw_rate; // desired yaw rate while acro modestatic float acro_level_mix; // scales back roll, pitch and yaw inversely proportional to input from pilot// Filters#if FRAME_CONFIG == HELI_FRAME//static LowPassFilterFloat rate_roll_filter; // Rate Roll filter//static LowPassFilterFloat rate_pitch_filter; // Rate Pitch filter#endif // HELI_FRAME////////////////////////////////////////////////////////////////////////////////// Circle Mode / Loiter control////////////////////////////////////////////////////////////////////////////////Vector3f circle_center; // circle position expressed in cm from home location. x = lat, y = lon// angle from the circle center to the copter's desired location. Incremented at circle_rate / secondstatic float circle_angle;// the total angle (in radians) travelledstatic float circle_angle_total;// deg : how many times to circle as specified by mission commandstatic uint8_t circle_desired_rotations;static float circle_angular_acceleration; // circle mode's angular accelerationstatic float circle_angular_velocity; // circle mode's angular velocitystatic float circle_angular_velocity_max; // circle mode's max angular velocity// How long we should stay in Loiter Mode for mission scripting (time in seconds)static uint16_t loiter_time_max;// How long have we been loitering - The start time in millisstatic uint32_t loiter_time;////////////////////////////////////////////////////////////////////////////////// CH7 and CH8 save waypoint control////////////////////////////////////////////////////////////////////////////////// This register tracks the current Mission Command index when writing// a mission using Ch7 or Ch8 aux switches in flightstatic int8_t aux_switch_wp_index;////////////////////////////////////////////////////////////////////////////////// Battery Sensors////////////////////////////////////////////////////////////////////////////////static AP_BattMonitor battery;////////////////////////////////////////////////////////////////////////////////// Altitude////////////////////////////////////////////////////////////////////////////////// The (throttle) controller desired altitude in cmstatic float controller_desired_alt;// The cm we are off in altitude from next_WP.alt – Positive value means we are below the WP static int32_t altitude_error;// The cm/s we are moving up or down based on filtered data - Positive = UPstatic int16_t climb_rate;// The altitude as reported by Sonar in cm – Values are 20 to 700 generally.static int16_t sonar_alt;static uint8_t sonar_alt_health; // true if we can trust the altitude from the sonarstatic float target_sonar_alt; // desired altitude in cm above the ground// The altitude as reported by Baro in cm – Values can be quite highstatic int32_t baro_alt;static int16_t saved_toy_throttle;////////////////////////////////////////////////////////////////////////////////// flight modes////////////////////////////////////////////////////////////////////////////////// Flight modes are combinations of Roll/Pitch, Yaw and Throttle control modes// Each Flight mode is a unique combination of these modes//// The current desired control scheme for Yawstatic uint8_t yaw_mode;// The current desired control scheme for roll and pitch / navigationstatic uint8_t roll_pitch_mode;// The current desired control scheme for altitude holdstatic uint8_t throttle_mode;////////////////////////////////////////////////////////////////////////////////// flight specific////////////////////////////////////////////////////////////////////////////////// An additional throttle added to keep the copter at the same altitude when banking。
apm32f103cbt6例程
STM32F103CBT6是意法半导体推出的一款基于ARM Cortex-M3内核的低功耗、高性能微控制器。
它拥有丰富的外设和接口，适用于各种嵌入式系统的开发。
而APM32F103CBT6则是基于STM32F103CBT6的一款开发板，它常用于嵌入式系统的原型设计和学习开发。
在使用APM32F103CBT6开发板时，绕不开的一个重要工作就是编写例程，通过例程的学习和实践，可以更好地了解开发板的功能和性能，同时也为后续的项目开发提供了基础。
本文将介绍关于APM32F103CBT6例程的相关知识和编写方法，希望能为广大开发者提供一些参考和帮助。
一、例程概述APM32F103CBT6的例程是针对该开发板的功能和特性编写的一系列代码程序，主要用于演示和测试开发板的各种功能。
可以通过例程来学习LED灯控制、按键输入、定时器应用、串口通信等基本功能，也可以结合外设模块如LCD、ADC、PWM等进行更丰富的应用开发。
二、例程编写工具在编写APM32F103CBT6的例程时，需要准备相应的开发工具。
常用的开发工具包括Keil、IAR、CubeMX等。
其中Keil是针对ARM系列微控制器专门设计的集成开发环境（IDE），具有强大的编译、调试和仿真功能，适合初学者和专业开发人员使用。
三、例程编写步骤编写APM32F103CBT6的例程一般包括以下几个步骤：1. 初始化配置：首先需要对芯片的时钟、IO口、外设等进行初始化配置，以便后续的功能使用。
2. 功能实现：根据需求，逐步实现各种功能，如LED灯控制、按键检测、定时器应用等。
3. 调试优化：在程序编写完成后，需要进行调试和优化，确保程序能够正确运行和稳定工作。
4. 扩展应用：在掌握基本功能后，可以结合外设模块进行更丰富的应用开发，如LCD显示、ADC采集、PWM调节等。
5. 文档整理：最后需要对例程编写过程进行总结和整理，形成详细的文档，方便后续的学习和使用。
四、例程应用实例下面以LED灯控制为例，介绍APM32F103CBT6的例程编写方法：1. 初始化配置：首先需要对GPIO口进行初始化配置，将LED连接的IO口设置为输出模式。
源代码分析
源代码分析pixhawk源码笔记一：apm代码基本结构pixhawk源码笔记一：apm代码基本结构基础知识第一部分：介绍toolsdirectoriesexternalsupportcode1、vehicledirectories模型类型当前共计4种模型：arduplane,arducopter,apmrover2andantennatracker。
都就是.pde文件，就是为了相容arduino平台，以后可能会退出。
2、ap_hal硬件抽象层硬件抽象化层，使在相同硬件平台上的移殖显得直观。
其中ap_hal目录定义了一个通用的接口。
其他的目录ap_hal_xxx针对不同硬件平台进行详细的定义。
例如ap_hal_avr目录对于avr平台，ap_hal_px4对应px4平台，ap_hal_linux对应linux平台。
3、toolsdirectories工具目录对于其他平台，需要外部支持代码。
例如pixhawk、px4的支持代码如下：px4nuttxc 板载实时系统。
thecorenuttxrtosusedonpx4boardspx4firmwarecpx4固件。
thebasepx4middlewareanddriversusedonpx4boardsuavcanc 飞行器can通信协议。
theuavcancanbusimplementationusedinardupilotmavlinkcmavlink通信协议。
themavlinkprotocolandcodegenerator5、系统编译针对相同的硬件板，编程可以使用“maketarget”的形式。
makeapm1ctheapm1boardmakeapm2ctheapm2boardmakepx4-v1cthepx4v1makepx4-v2cthepixhawk如果必须移殖至代莱硬件，可以在mk/targets.mk文件中嵌入。
比如说：makeapm2-octa-j8或者：makepx4-v2-j8采用8通道并行编译方式，针对apm、pixhawk硬件板（avr、stm32），编译八旋翼代码。
apm ekf代码
apm ekf代码
关于APM（ArduPilot Mega）的EKF（扩展卡尔曼滤波）代码，它是开源的飞行控制系统，主要用于多旋翼、固定翼和其他类型的自动驾驶飞行器。
APM的EKF代码主要用于传感器融合，以提高飞行器的姿态估计和导航性能。
APM的EKF代码主要包括以下几个方面：
1. 传感器数据融合，EKF代码会接收来自飞行器上安装的各种传感器（如加速度计、陀螺仪、磁力计、气压计等）的数据，并利用EKF算法将这些数据融合起来，以估计飞行器的姿态和位置。
2. 姿态估计，EKF代码会根据传感器数据融合的结果，估计飞行器的姿态（即俯仰、横滚和偏航角），从而帮助飞行控制系统稳定飞行器并执行各种飞行任务。
3. 导航性能，除了姿态估计，EKF代码还会利用传感器数据融合的结果，估计飞行器的位置、速度和加速度，以实现自主导航和飞行任务执行。
在APM项目的代码库中，EKF相关的代码通常位于姿态控制和导航模块中，主要由C++语言编写。
这些代码经过了开发者社区的不断优化和更新，以适应不同类型飞行器的需求，并提高飞行器的飞行性能和稳定性。
总的来说，APM的EKF代码是飞行控制系统中至关重要的一部分，它通过传感器数据融合和状态估计，为飞行器提供精准的姿态和位置信息，从而实现自主飞行和各种飞行任务的执行。
详细的APM飞控调试资料
详细的A P M飞控调试资料文件管理序列号：[K8UY-K9IO69-O6M243-OL889-F88688]调整ArduCopter?参数如果你使用的机身不是官方 ArduCopter 套件，你可能需要改变一些PID设置（PID 是比例-积分- 微分的简称，是一个标准的控制方法。
更多的资料在这里）。
在此页底部的有一个PID的全面的指导。
你可以在任务规划器的配置选项卡中以交互方式调整 PID：基本性能故障排除我的多旋翼在稳定模式下缓慢震荡（大幅运动）:?降低?STABILIZE_ROLL_P, STABILIZE_PITCH_P.?我的多旋翼在稳定模式下***震荡（小幅运动）:?降低?RATE_ROLL_P, RATE_PITCH_P.?我的飞机过于迟钝:?降低?RATE_ROLL_P, RATE_PITCH_P,?和/或增加?STABILIZE_ROLL_P,? ???STABILIZE_PITCH_P.?我调整了?Rate_P，还是不行:?也许你的?STABILIZE_P gain?增益过高。
降低一点（见上文），并再次尝试调整?RATE_P.?我的飞机在起飞时向左或向右旋转15°：你的电机不直或着电调没有校准。
扭转电机，直到他们都直了。
运行ESC校准程序。
?激烈飞行后我的飞机偏向一方?10 - 30°：?如该文所述，焊接?IMU?的滤波器U。
你可以在?system.pde?里调整漂移校正。
如果需要，大概调高0.5。
此外，降落30秒，然后继续飞行。
?我的飞机无法在空中保持完全静止：?确保在飞机的重心在正中心。
然后在水平面上运行水平命令（保持关闭状态15秒，调用该功能）。
你也可以在无风的环境（重要）使用自动微调模式飞行。
任何风将导致四轴旋转180度后你的修改产生相反的作用。
你可以使用遥控俯仰和横滚微调，但记得在用配置工具设置遥控时，要把它们放回中心。
我不喜欢使用发射微调，但永远不要使用偏航微调。
APM飞控源码讲解
APM飞控源码讲解
APM（ArduPilot Mega）是一款开源的飞控系统，其源码可以在
GitHub上找到。
本文将对APM飞控系统的源码进行讲解。
在飞行控制模块中，最重要的是飞行姿态控制算法。
源码中实现了多
种不同的姿态控制算法，包括经典的PID控制器和先进的模型预测控制器。
这些算法通过读取飞行器的传感器数据，如加速度计和陀螺仪，来计算飞
行器当前的姿态，并根据目标姿态进行调整。
源码中还实现了飞行器的导
航控制算法，通过GPS数据和地面站指令，实现导航目标的控制。
另一个重要的模块是传感器数据采集模块，用于获取飞行器的传感器
数据。
源码中实现了对加速度计、陀螺仪、磁力计等传感器的读取和数据
处理。
通过对传感器数据的处理，可以获取飞行器的姿态、加速度、角速
度等信息，用于姿态控制和导航控制。
通信模块负责与地面站进行通信，传输飞行器的状态和控制指令。
源
码中实现了多种通信方式，包括串口通信和无线通信，以适应不同的应用
场景。
通过与地面站的通信，可以实时监控飞行器的状态，并发送控制指
令进行调整。
此外，源码中还包括一些辅助功能模块，如传感器校准、飞行任务管理、参数配置等。
这些模块可以帮助用户对飞行器进行配置和管理，提供
更加精确和可靠的飞行控制。
总之，APM飞控系统的源码实现了一套完整的飞行控制系统，包括姿
态控制、传感器数据采集、通信等关键功能。
通过对源码的深入理解和学习，可以帮助开发者更好地应用和开发飞行控制系统，实现更高级的飞行
功能和应用。
apm飞控通信协议v
apm飞控通信协议vAPM飞控通信协议V2.0简介APM（ArduPilotMega）飞控是一种开源的飞行控制系统，被广泛应用于自动驾驶无人机和机器人领域。
为了实现与外部设备的通信和数据交换，APM采用了特定的通信协议。
本文将介绍APM飞控通信协议V2.0的详细内容。
通信协议概述APM飞控通信协议V2.0是一种二进制协议，用于在飞控系统和其他设备之间传输数据。
该协议基于串行通信，并支持广播和点对点通信模式。
在通信过程中，数据被打包成帧，通过串口进行传输。
帧格式APM飞控通信协议V2.0的帧格式如下：帧头（2字节）：标识一帧数据的开始，固定为0x55AA。
帧长度（2字节）：指示整帧数据的长度，包括校验位。
数据（n字节）：具体的数据内容，长度根据实际需要而定。
校验位（2字节）：通过对帧长度和数据进行校验得到的结果，用于验证数据的完整性。
数据类型APM飞控通信协议V2.0支持多种数据类型的传输，包括命令、响应、参数和日志等。
具体的数据类型及其对应的标识如下：命令数据：用于向飞控发送指令，标识为0x01。
响应数据：用于表示飞控对指令的响应结果，标识为0x02。
参数数据：用于获取和修改飞控的参数，标识为0x03。
日志数据：用于记录和传输飞行过程中的日志信息，标识为0x04。
通信流程APM飞控通信协议V2.0的通信流程如下：发送方构建数据帧，设置帧头、帧长度、数据和校验位。
发送方通过串口将数据帧发送给接收方。
接收方接收数据帧，并进行校验，确保数据完整。
接收方解析数据帧，根据帧类型进行相应的处理。
接收方根据需要构建响应数据帧，并将其发送给发送方。
发送方接收响应数据帧，并进行校验和解析。
发送方根据响应结果进行下一步操作或继续通信。
实例演示以下是一个以点对点通信模式为例的APM飞控通信协议V2.0实例：发送方发送命令数据帧给接收方。
接收方接收并解析命令数据帧，执行相应的指令。
接收方构建响应数据帧，将执行结果发送给发送方。
APM飞行模式注解
A P M飞行模式注解(总2页)--本页仅作为文档封面，使用时请直接删除即可----内页可以根据需求调整合适字体及大小--APM飞行模式注解1、稳定模式Stabilize稳定模式是使用得最多的飞行模式，也是最基本的飞行模式，起飞和降落都应该使用此模式。
此模式下，飞控会让飞行器保持稳定，是初学者进行一般飞行的首选，也是FPV第一视角飞行的最佳模式。
一定要确保遥控器上的开关能很方便无误地拨到该模式，应急时会非常重要。
2、比率控制模式Acro这个是非稳定模式,这时apm将完全依托遥控器遥控的控制，新手慎用。
3、定高模式ALT_HOLD定高模式(Alt Hold)是使用自动油门，试图保持目前的高度的稳定模式。
定高模式时高度仍然可以通过提高或降低油门控制，但中间会有一个油门死区，油门动作幅度超过这个死区时，飞行器才会响应你的升降动作当进入任何带有自动高度控制的模式，你目前的油门将被用来作为调整油门保持高度的基准。
在进入高度保持前确保你在悬停在一个稳定的高度。
飞行器将随着时间补偿不良的数值。
只要它不会下跌过快，就不会有什么问题。
离开高度保持模式时请务必小心，油门位置将成为新的油门，如果不是在飞行器的中性悬停位置，将会导致飞行器迅速下降或上升。
在这种模式下你不能降落及关闭马达，因为现在是油门摇杆控制高度，而非马达。
请切换到稳定模式，才可以降落和关闭马达。
4、悬停模式Loiter悬停模式是GPS定点+气压定高模式。
应该在起飞前先让GPS定点，避免在空中突然定位发生问题。
其他方面跟定高模式基本相同，只是在水平方向上由GPS 进行定位。
5、简单模式Simple Mode简单模式相当于一个无头模式，每个飞行模式的旁边都有一个Simple Mode复选框可以勾选。
勾选简单模式后，飞机将解锁起飞前的机头指向恒定作为遥控器前行摇杆的指向，这种模式下无需担心飞行器的姿态，新手非常有用。
6、自动模式 AUTO自动模式下，飞行器将按照预先设置的任务规划控制它的飞行由于任务规划依赖GPS的定位信息，所以在解锁起飞前，必须确保GPS已经完成定位（APM板上蓝色LED常亮）切换到自动模式有两种情况：如果使用自动模式从地面起飞，飞行器有一个安全机制防止你误拨到自动模式时误启动发生危险，所以需要先手动解锁并手动推油门起飞。
apm32f003例程
apm32f003例程APM32F003是一款高性能的32位微控制器，具有广泛的应用领域。
本文将介绍APM32F003的例程，包括其功能、使用方法以及一些相关的注意事项。
APM32F003例程是为了帮助开发者快速上手使用该微控制器而设计的。
它提供了一系列的示例代码和应用案例，旨在展示APM32F003的各种功能和特性。
APM32F003例程中包含了一些基本的功能示例，如GPIO控制、定时器和中断处理等。
这些示例代码可以帮助开发者了解如何通过APM32F003来控制外部设备和实现特定的功能。
例如，通过GPIO控制示例，开发者可以学习如何使用APM32F003来控制LED灯的亮灭，或者控制蜂鸣器的声音。
除了基本的功能示例，APM32F003例程还包含了一些高级的应用案例，如通信协议的实现、模拟信号处理等。
这些应用案例可以帮助开发者深入了解APM32F003的各种功能和特性，并帮助他们在自己的项目中应用这些功能。
例如，通过通信协议的实现示例，开发者可以学习如何使用APM32F003来实现UART、SPI或I2C等通信协议，从而与其他设备进行数据交换。
在使用APM32F003例程时，开发者需要注意一些事项。
首先，他们需要确保自己已经熟悉了APM32F003的芯片规格和引脚定义，以便正确连接外部设备。
其次，开发者需要注意例程中的代码逻辑和参数设置，以确保代码的正确性和可靠性。
最后，开发者还需要注意例程中的一些特殊要求，如时序要求、电源要求等，以确保系统的正常运行。
APM32F003例程是一个非常有用的资源，可以帮助开发者快速上手使用APM32F003微控制器。
通过学习和实践这些例程，开发者可以更好地理解APM32F003的各种功能和特性，并将其应用到自己的项目中。
希望本文对读者有所帮助，感谢阅读。
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
APM飞控系统介绍APM飞控系统是国外的一个开源飞控系统，能够支持固定翼，直升机，3轴，4轴，6轴飞行器。
在此我只介绍固定翼飞控系统。
APM飞控系统主要结构和功能组成功能飞控主芯片Atmega1280/2560 主控芯片PPM解码芯片Atmega168/328 负责监视模式通道的pwm信号监测，以便在手动模式和其他模式之间进行切换。
提高系统安全惯性测量单元双轴陀螺，单轴陀螺，三轴加速度计测量三轴角速度，三轴加速度，配合三轴磁力计或gps测得方向数据进行校正，实现方向余弦算法，计算出飞机姿态。
GPS导航模块Lea-5h或其他信号gps模块测量飞机当前的经纬度，高度，航迹方向（track），地速等信息。
三轴磁力计模块HMC5843/5883模块测量飞机当前的航向（heading）空速计MPXV7002模块测量飞机空速（误差较大，而且测得数据不稳定，会导致油门一阵一阵变化）空压计BMP085芯片测量空气压力，用以换算成高度AD芯片ADS7844芯片将三轴陀螺仪、三轴加速度计、双轴陀螺仪输出温度、空速计输出的模拟电压转换成数字量，以供后续计算其他模块电源芯片，usb电平转换芯片等飞控原理在APM飞控系统中，采用的是两级PID控制方式，第一级是导航级，第二级是控制级，导航级的计算集中在medium_loop( ) 和fastloop( )的update_current_flight_mode( )函数中，控制级集中在fastloop( )的stabilize( )函数中。
导航级PID控制就是要解决飞机如何以预定空速飞行在预定高度的问题，以及如何转弯飞往目标问题，通过算法给出飞机需要的俯仰角、油门和横滚角，然后交给控制级进行控制解算。
控制级的任务就是依据需要的俯仰角、油门、横滚角，结合飞机当前的姿态解算出合适的舵机控制量，使飞机保持预定的俯仰角，横滚角和方向角。
最后通过舵机控制级set_servos_4( )将控制量转换成具体的pwm信号量输出给舵机。
值得一提的是，油门的控制量是在导航级确定的。
控制级中不对油门控制量进行解算，而直接交给舵机控制级。
而对于方向舵的控制，导航级并不给出方向舵量的解算，而是由控制级直接解算方向舵控制量，然后再交给舵机控制级。
以下，我剔除了APM飞控系统的细枝末节，仅仅将飞控系统的重要语句展现，只浅显易懂地说明APM飞控系统的核心工作原理。
一，如何让飞机保持预定高度和空速飞行要想让飞机在预定高度飞行，飞控必须控制好飞机的升降舵和油门，因此，首先介绍固定翼升降舵和油门的控制，固定翼的升降舵和油门控制方式主要有两种：一种是高度控制油门，空速控制升降舵方式。
实际飞行存在四种情况，第一种情况是飞机飞行过程中，如果高度低于目标高度，飞控就会控制油门加大，从而导致空速加大，然后才导致拉升降舵，飞机爬升；第二种情况与第一种情况相反；第三种情况是飞机在目标高度，但是空速高于目标空速，这种情况飞控会直接拉升降舵，使飞机爬升，降低空速，但是，高度增加了，飞控又会减小油门，导致空速降低，空速低于目标空速后，飞控推升降舵，导致飞机降低高度。
这种控制方式的好处是，飞机始终以空速为第一因素来进行控制，因此保证了飞行的安全，特别是当发动机熄火等异常情况发生时，使飞机能继续保持安全，直到高度降低到地面。
这种方式的缺点在于对高度的控制是间接控制，因此高度控制可能会有一定的滞后或者波动。
另一种是高度控制升降舵，空速控制油门的方式。
这种控制方式的原理是设定好飞机平飞时的迎角，当飞行高度高于或低于目标高度时，在平飞迎角的基础上根据高度与目标高度的差设定一个经过PID控制器输出的限制幅度的爬升角，由飞机当前的俯仰角和爬升角的偏差来控制升降舵面，使飞机迅速达到这个爬升角，而尽快完成高度偏差的消除。
但飞机的高度升高或降低后，必然造成空速的变化，因此采用油门来控制飞机的空速，即当空速低于目标空速后，在当前油门的基础上增加油门，当前空速高于目标空速后，在当前油门的基础上减小油门。
这种控制方式的好处是能对高度的变化进行第一时间的反应，因此高度控制较好，缺点是当油门失效时，比如发动机熄火发生时，由于高度降低飞控将使飞机保持经过限幅的最大仰角，最终由于动力的缺乏导致失速。
但是以上仅仅是控制理论。
在实际控制系统中，由于有些参量并不能较准确地测得，或者测量时数据不稳定，所以并不能完全按照上述的控制理论控制。
例如空速的测量时相当不准确的，而且数据波动较严重，这样，就无法完全按照上述理论进行控制，必须在其基础上进行适当修改。
以下以使用空速计情况和不使用空速计情况对APM飞控系统进行阐述。
（1），使用空速计情况在使用空速计的情况下，升降舵是由空速控制。
update_current_flight_mode( )调用calc_nav_pitch( )调用nav_pitch = -g.pidNavPitchAirspeed.get_pid(airspeed_error, dTnav)。
nav_pitch就是导航俯仰角，也就是说，使用空速计时，APM系统对利用空速偏差airspeed_error作为输入量进行导航级的俯仰角控制。
在使用空速计的情况下，油门是由飞机机械能偏差控制，也就是空速误差和高度误差共同决定。
update_current_flight_mode( )调用calc_throttle( )调用g.channel_throttle.servo_out = g.throttle_cruise + g.pidTeThrottle.get_pid(energy_error, dTnav);g.channel_throttle.servo_out += (g.channel_pitch.servo_out * g.kff_pitch_to_throttle);式中energy_error = airspeed_energy_error + (float )altitude_error * 0.098f ，是空速动能偏差，加上飞机重力势能偏差。
可以看出，油门是由设定的巡航油门g.throttle_cruise 、机械能偏差PID 调节量和升降舵通道补偿共同决定，但是巡航油门是设定值，是固定的。
g.kff_pitch_to_throttle 默认是0，所以，实际上油门的增减是由机械能偏差控制的。
所以，使用空速计时，APM 飞控系统的油门升降舵控制属于空速控制升降，机械能控制油门方案，类似于第一种控制方案，但是又有点区别。
（2），不使用空速计情况不使用空速计时，升降舵是由高度偏差控制。
update_current_flight_mode( )中调用calc_nav_pitch( )调用nav_pitch = g.pidNavPitchAltitude.get_pid(altitude_error, dTnav)。
所以升降舵的控制，是由高度误差altitude_error 作为PID 调节的输入量。
不使用空速计时，油门是由导航俯仰角控制。
update_current_flight_mode( )调用calc_throttle( )调用if (nav_pitch >= 0){g.channel_throttle.servo_out = throttle_target + (g.throttle_max - throttle_target)* nav_pitch / g.pitch_limit_max;}else{g.channel_throttle.servo_out = throttle_target - (throttle_target - g.throttle_min)* nav_pitch / g.pitch_limit_min;}可以看出此时的油门控制是利用的是比例调节，依据的比例关系是目标油门最大油门油门增减量-=导航俯仰角变化量导航俯仰角。
二，如何让飞机飞往目标要使飞机飞往目标，那就必须知道飞机当前位置、目标位置和当前航向等问题。
在APM 飞控系统中，GPS 模块能够提供飞机当前经纬度信息，航迹方向和地速信息。
根据这些信息，再用程序解算飞机当前位置和目标位置的关系，就能知道目标航向角target_bearing ，知道了目标航向角target_bearing 后就可以用于引导飞机飞向目标。
但是仅用目标航向角进行导航，不能压航线飞行，为了解决这个问题，APM 飞控系统中又增加了偏航距crosstrack_error 的计算，并且根据偏航距，计算出需要的偏航修正量crosstrack_error * g.crosstrack_gain 。
使飞机能尽快飞到航线上。
最后把目标航向角和偏航修正量组成导航航向角nav_bearing ，提供给控制级PID 。
所以目标航向角的计算和偏航修正量的计算是构成如何让飞机飞往目标的核心。
下面具体介绍APM 中关于这部分的程序。
APM 飞控系统中的GPS 信息只能每秒更新4-10次。
所以，计算目标航向角和偏航修正量的程序都在每秒大约执行10次的medium_loop( )中。
在medium_loop( ) 的case 1中会执行navigate( )，正是在这个函数中，执行了导航航向角nav_bearing的计算。
首先计算的是目标航向角。
在navigate( )中有：target_bearing = get_bearing(¤t_loc, &next_WP);nav_bearing = target_bearing;第一个语句中current_loc和next_WP是结构体，里面存储这一个位置点的经度、纬度、高度信息，current_lot中存储的是当前点，next_WP中存储的是目标点。
根据这个进行在球体表面的三角函数计算（此文中，由于篇幅所限，很多东西不进行详细讲解），就可以得出目标航向target_bearing。
接下来，要计算偏航修正量。
navigate( )调用update_navigation( )调用verify_commands( )调用verify_nav_wp( )调用update_crosstrack( )，这个函数中有：crosstrack_error = sin(radians((target_bearing - crosstrack_bearing) / 100)) * wp_distance; nav_bearing += constrain(crosstrack_error * g.crosstrack_gain, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get());第一句是计算偏航距的，偏航距是飞机当前位置点到航线的距离，事实上就是求一个点到一条线之间的距离。