ArduPilot代码学习--模式切换

最新推荐文章于 2025-07-02 08:42:46 发布

Chasing中的小强

最新推荐文章于 2025-07-02 08:42:46 发布

阅读量6.4k

点赞数 3

CC 4.0 BY-SA版权

分类专栏： ArduPilot 个人总结文章标签： c++

本文链接：https://blog.youkuaiyun.com/qq_15390133/article/details/106311555

引言：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(