ArduPilot代码学习–模式切换javascriptqq15390133的博客-

引言：ArduPilot代码兼容无人机，无人车，UUV，帆船等多种vehicle，本文以Copter为例，说明代码中是如何完成模式切换的。

各个模式的init

首先定位在系统初始化中，即system.cpp中的 rc().init(); 执行跳转查看函数
init_aux_all(); –>reset_mode_switch(); –>c->reset_mode_switch();–>read_mode_switch();

void RC_Channel::read_mode_switch() { // calculate position of flight mode switch const uint16_t pulsewidth = get_radio_in(); if (pulsewidth <= 900 || pulsewidth >= 2200) { return; // This is an error condition }      modeswitch_pos_t position; if (pulsewidth < 1231) position = 0; else if (pulsewidth < 1361) position = 1; else if (pulsewidth < 1491) position = 2; else if (pulsewidth < 1621) position = 3; else if (pulsewidth < 1750) position = 4; else position = 5; if (!debounce_completed(position)) { return; } // set flight mode and simple mode setting mode_switch_changed(position); } 

mode_switch_changed为虚函数，在Copter中进行了实例化。

void RC_Channel_Copter::mode_switch_changed(modeswitch_pos_t new_pos) { if (new_pos < 0 || (uint8_t)new_pos > copter.num_flight_modes) { // should not have been called return; } if (!copter.set_mode((Mode::Number)copter.flight_modes[new_pos].get(), ModeReason::RC_COMMAND)) { // alert user to mode change failure if (copter.ap.initialised) {             AP_Notify::events.user_mode_change_failed = 1; } return; } // play a tone // alert user to mode change (except if autopilot is just starting up) if (copter.ap.initialised) {         AP_Notify::events.user_mode_change = 1; } if (!rc().find_channel_for_option(AUX_FUNC::SIMPLE_MODE) && !rc().find_channel_for_option(AUX_FUNC::SUPERSIMPLE_MODE)) { // if none of the Aux Switches are set to Simple or Super Simple Mode then // set Simple Mode using stored parameters from EEPROM if (BIT_IS_SET(copter.g.super_simple, new_pos)) {             copter.set_simple_mode(2); } else {             copter.set_simple_mode(BIT_IS_SET(copter.g.simple_modes, new_pos)); } } } 

其中下图所示内容为关键点：

Mode::Number为模式的所有类别。

enum class Number : uint8_t {     STABILIZE = 0, // manual airframe angle with manual throttle     ACRO = 1, // manual body-frame angular rate with manual throttle     ALT_HOLD = 2, // manual airframe angle with automatic throttle     AUTO = 3, // fully automatic waypoint control using mission commands     GUIDED = 4, // fully automatic fly to coordinate or fly at velocity/direction using GCS immediate commands     LOITER = 5, // automatic horizontal acceleration with automatic throttle     RTL = 6, // automatic return to launching point     CIRCLE = 7, // automatic circular flight with automatic throttle     LAND = 9, // automatic landing with horizontal position control     DRIFT = 11, // semi-automous position, yaw and throttle control     SPORT = 13, // manual earth-frame angular rate control with manual throttle     FLIP = 14, // automatically flip the vehicle on the roll axis     AUTOTUNE = 15, // automatically tune the vehicle's roll and pitch gains     POSHOLD = 16, // automatic position hold with manual override, with automatic throttle     BRAKE = 17, // full-brake using inertial/GPS system, no pilot input     THROW = 18, // throw to launch mode using inertial/GPS system, no pilot input     AVOID_ADSB = 19, // automatic avoidance of obstacles in the macro scale - e.g. full-sized aircraft     GUIDED_NOGPS = 20, // guided mode but only accepts attitude and altitude     SMART_RTL = 21, // SMART_RTL returns to home by retracing its steps     FLOWHOLD  = 22, // FLOWHOLD holds position with optical flow without rangefinder     FOLLOW    = 23, // follow attempts to follow another vehicle or ground station     ZIGZAG    = 24, // ZIGZAG mode is able to fly in a zigzag manner with predefined point A and point B     SYSTEMID  = 25, // System ID mode produces automated system identification signals in the controllers     AUTOROTATE = 26, // Autonomous autorotation }; 

而SetMode函数如下，其中关键语句如下图所示：

// set_mode - change flight mode and perform any necessary initialisation // optional force parameter used to force the flight mode change (used only first time mode is set) // returns true if mode was successfully set // ACRO, STABILIZE, ALTHOLD, LAND, DRIFT and SPORT can always be set successfully but the return state of other flight modes should be checked and the caller should deal with failures appropriately bool Copter::set_mode(Mode::Number mode, ModeReason reason) { // return immediately if we are already in the desired mode if (mode == control_mode) {         control_mode_reason = reason; return true; }      Mode *new_flightmode = mode_from_mode_num((Mode::Number)mode); if (new_flightmode == nullptr) { gcs().send_text(MAV_SEVERITY_WARNING,"No such mode");         AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode)); return false; } bool ignore_checks = !motors->armed(); // allow switching to any mode if disarmed.  We rely on the arming check to perform #if FRAME_CONFIG == HELI_FRAME // do not allow helis to enter a non-manual throttle mode if the // rotor runup is not complete if (!ignore_checks && !new_flightmode->has_manual_throttle() && (motors->get_spool_state() == AP_Motors::SpoolState::SPOOLING_UP || motors->get_spool_state() == AP_Motors::SpoolState::SPOOLING_DOWN)) { #if MODE_AUTOROTATE_ENABLED == ENABLED //if the mode being exited is the autorotation mode allow mode change despite rotor not being at //full speed.  This will reduce altitude loss on bail-outs back to non-manual throttle modes bool in_autorotation_check = (flightmode != &mode_autorotate || new_flightmode != &mode_autorotate); #else bool in_autorotation_check = false; #endif if (!in_autorotation_check) { gcs().send_text(MAV_SEVERITY_WARNING,"Flight mode change failed");             AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode)); return false; } } #endif #if FRAME_CONFIG != HELI_FRAME // ensure vehicle doesn't leap off the ground if a user switches // into a manual throttle mode from a non-manual-throttle mode // (e.g. user arms in guided, raises throttle to 1300 (not enough to // trigger auto takeoff), then switches into manual): bool user_throttle = new_flightmode->has_manual_throttle(); #if MODE_DRIFT_ENABLED == ENABLED if (new_flightmode == &mode_drift) {         user_throttle = true; } #endif if (!ignore_checks &&         ap.land_complete &&         user_throttle && !copter.flightmode->has_manual_throttle() &&         new_flightmode->get_pilot_desired_throttle() > copter.get_non_takeoff_throttle()) { gcs().send_text(MAV_SEVERITY_WARNING, "Mode change failed: throttle too high");         AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode)); return false; } #endif if (!ignore_checks &&         new_flightmode->requires_GPS() && !copter.position_ok()) { gcs().send_text(MAV_SEVERITY_WARNING, "Mode change failed: %s requires position", new_flightmode->name());         AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode)); return false; } if (!new_flightmode->init(ignore_checks)) { gcs().send_text(MAV_SEVERITY_WARNING,"Flight mode change failed");         AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode)); return false; } // perform any cleanup required by previous flight mode exit_mode(flightmode, new_flightmode); // store previous flight mode (only used by tradeheli's autorotation)     prev_control_mode = control_mode; // update flight mode     flightmode = new_flightmode;     control_mode = mode;     control_mode_reason = reason;     logger.Write_Mode((uint8_t)control_mode, reason); gcs().send_message(MSG_HEARTBEAT); #if ADSB_ENABLED == ENABLED     adsb.set_is_auto_mode((mode == Mode::Number::AUTO) || (mode == Mode::Number::RTL) || (mode == Mode::Number::GUIDED)); #endif #if AC_FENCE == ENABLED // pilot requested flight mode change during a fence breach indicates pilot is attempting to manually recover // this flight mode change could be automatic (i.e. fence, battery, GPS or GCS failsafe) // but it should be harmless to disable the fence temporarily in these situations as well     fence.manual_recovery_start(); #endif #if CAMERA == ENABLED     camera.set_is_auto_mode(control_mode == Mode::Number::AUTO); #endif // update notify object notify_flight_mode(); // return success return true; } 

即完成了对应模式的初始化工作。

各个模式的Run

首先定位到Copter.cpp中，找到fast_loop中的update_flight_mode(); 可以看到里面即可以定位到模式的Run。

// update_flight_mode - calls the appropriate attitude controllers based on flight mode // called at 100hz or more void Copter::update_flight_mode() {     surface_tracking.invalidate_for_logging(); // invalidate surface tracking alt, flight mode will set to true if used      flightmode->run(); } 

同时，在rc_loop中实时查看模式的切换

// rc_loops - reads user input from transmitter/receiver // called at 100hz void Copter::rc_loop() { // Read radio and 3-position switch on radio // ----------------------------------------- read_radio(); rc().read_mode_switch(); } 

通过rc().read_mode_switch(); –> c->read_mode_switch(); 随后执行就与上文所提到的初始化进入了相同的地方。

注明

下图所示代码位于SetMode函数中，表示判断是否可以进行模式切换功能，具体含义参看if判断。同时如果切换成功，则进入exit_mode函数中，该函数完成模式的过渡过程。

参考：Ardupilot 高度控制代码整理(超长篇)