ArduPilot代码学习--模式切换
引言：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函数中，该函数完成模式的过渡过程。
