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飞行模式注解

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常亮）切换到自动模式有两种情况：如果使用自动模式从地面起飞，飞行器有一个安全机制防止你误拨到自动模式时误启动发生危险，所以需要先手动解锁并手动推油门起飞。

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飞控源码讲解

10次的medium_loop( )中。在medium_loop( ) 的case 1
navigate( )，正是在这个函数中，执行了导航航向角nav_bearing的计算。
navigate( )中有：
= get_bearing(&current_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。

1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。

APM飞控系统介绍APM飞控系统是国外的一个开源飞控系统，能够支持固定翼，直升机，3轴，4轴，6轴飞行器。

在此我只介绍固定翼飞控系统。

飞控原理在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(&current_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());第一句是计算偏航距的，偏航距是飞机当前位置点到航线的距离，事实上就是求一个点到一条线之间的距离。

wp_distance是这个直角三角形的斜边，target_bearing - crosstrack_bearing正是偏航距对应的边相对的那个锐角。

第二句中crosstrack_error * g.crosstrack_gain使用偏航距乘以偏航修正增益就得出需要的偏航距修正量，然后使用constrain( )函数将偏航距修正量限制在-g.crosstrack_entry_angle.get()与g.crosstrack_entry_angle.get()之间。

g.crosstrack_entry_angle.get()其实就是最大的偏航距修正量。

在上一段中target_bearing计算时已经有nav_bearing = target_bearing。

现在又nav_bearing += constrain(crosstrack_error * g.crosstrack_gain, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get())，这样其实就把目标航向角和偏航距修正都加到了nav_bearing 中。