ardupilot 关于设备车Rover的学习《3》------模式控制

目录

---

摘要

---

本节主要记录ardupilot中小车rover代码是如何进行模式更新的，欢迎批评指正！

---

1.模式简介

---

rover中主要用的模式如下：

    //自动飞行模式----- Auto Pilot modes
    // ----------------
    enum Number : uint8_t {
        MANUAL       = 0,  //手动模式
        ACRO         = 1,  //速率模式
        STEERING     = 3,  //转向模式
        HOLD         = 4,  //保持模式
        LOITER       = 5,  //悬停模式
        FOLLOW       = 6,  //跟随模式
        SIMPLE       = 7,  //简单模式
        AUTO         = 10, //自动模式
        RTL          = 11, //返航模式
        SMART_RTL    = 12, //智能返航模式
        GUIDED       = 15, //引导模式
        INITIALISING = 16  //初始化模式
    };

这种主要比较常用的模式：手动模式 ，保持模式 ，悬停模式，自动模式，返航模式。

---

2.如何设置模式

---

一般设置模式主要是通过模式开关5通道来实现，也可以通过辅助开关实现，本节主要讲解通过5通道模式开关来实现。
打开MP地面站通过MP地面站直接设置，或者通过全局参数来实现模式的设置。

---

3.模式代码实现

---

    SCHED_TASK_CLASS(RC_Channels,         (RC_Channels*)&rover.g2.rc_channels, read_mode_switch,        7,    200,  57), //读取模式开关

读取模式开关

void RC_Channels::read_mode_switch()
{
	//判断遥控是否有效
    if (!has_valid_input()) 
    {
        //如果没有遥控器输入立即退出------ exit immediately when no RC input
        return;
    }
    //读取遥控器模式通道值
    RC_Channel *c = flight_mode_channel();
    //没有设置，立即返回
    if (c == nullptr) 
    {
        return;
    }
    //数据读取
    c->read_mode_switch();
}

void RC_Channel::read_mode_switch()
{
    int8_t position;
    //读取6段开关的位置
    if (read_6pos_switch(position)) 
    {
        //设置飞行模式和简单模式设置---- set flight mode and simple mode setting
        mode_switch_changed(modeswitch_pos_t(position));
    }
}

根据开关位置设置对应的模式

void RC_Channel_Rover::mode_switch_changed(modeswitch_pos_t new_pos)
{
    if (new_pos < 0 || (uint8_t)new_pos > rover.num_modes) 
    {
        //直接返回---- should not have been called
        return;
    }
    //根据所在位置设置相对的模式
    rover.set_mode((Mode::Number)rover.modes[new_pos].get(), ModeReason::RC_COMMAND);
}

到这里最重要的就是实现函数了。

rover.set_mode((Mode::Number)rover.modes[new_pos].get(), ModeReason::RC_COMMAND);

bool Rover::set_mode(const uint8_t new_mode, ModeReason reason)
{
	//检查
    static_assert(sizeof(Mode::Number) == sizeof(new_mode), "The new mode can't be mapped to the vehicles mode number");
    //读取模式
    Mode *mode = rover.mode_from_mode_num((enum Mode::Number)new_mode);
    //是否为空
    if (mode == nullptr) 
    {
        notify_no_such_mode(new_mode);
        return false;
    }
    //设置进去
    return rover.set_mode(*mode, reason);
}

Mode *Rover::mode_from_mode_num(const enum Mode::Number num)
{
    Mode *ret = nullptr;
    switch (num) {
    case Mode::Number::MANUAL: //手动
        ret = &mode_manual;
        break;
    case Mode::Number::ACRO:  //速率
        ret = &mode_acro;
        break;
    case Mode::Number::STEERING: //转向
        ret = &mode_steering;
        break; 
    case Mode::Number::HOLD: //保持
        ret = &mode_hold;
        break;
    case Mode::Number::LOITER: //定点
        ret = &mode_loiter;
        break;
    case Mode::Number::FOLLOW: //跟随
        ret = &mode_follow;
        break;
    case Mode::Number::SIMPLE: //简单模式
        ret = &mode_simple;
        break;
    case Mode::Number::AUTO:  //自动模式
        ret = &mode_auto;
        break;
    case Mode::Number::RTL:   //返航模式
        ret = &mode_rtl;
        break;
    case Mode::Number::SMART_RTL: //智能返航模式
        ret = &mode_smartrtl;
        break;
    case Mode::Number::GUIDED:  //引导模式
        ret = &mode_guided;
        break;
    case Mode::Number::INITIALISING: //初始化模式
        ret = &mode_initializing;
        break;
    default:
        break;
    }
    return ret;
}

最后调用设置模式：

    //设置进去
    return rover.set_mode(*mode, reason);

bool Rover::set_mode(Mode &new_mode, ModeReason reason)
{
	//判断模式是否最新
    if (control_mode == &new_mode) 
    {
        //don't switch modes if we are already in the correct mode.
    	//如果我们已经处于正确的模式，请不要切换模式。
        return true;
    }
    //记录老的模式
    Mode &old_mode = *control_mode;
    //判断模式
    if (!new_mode.enter()) 
    {
        //Log error that we failed to enter desired flight mode
    	//日志错误：我们未能进入所需的飞行模式
        AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE,
                                 LogErrorCode(new_mode.mode_number()));
        gcs().send_text(MAV_SEVERITY_WARNING, "Flight mode change failed");
        return false;
    }

    control_mode = &new_mode;

    // 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
    //在围栏破裂期间，飞行员请求的飞行模式改变表明飞行员正试图手动恢复
    //这种飞行模式改变可以是自动的（即围栏、电池、GPS或GCS故障保护）
    //但在这些情况下暂时关闭围栏也是无害的
    g2.fence.manual_recovery_start();

#if CAMERA == ENABLED
    camera.set_is_auto_mode(control_mode->mode_number() == Mode::Number::AUTO);
#endif

    old_mode.exit();

    control_mode_reason = reason;
    logger.Write_Mode(control_mode->mode_number(), control_mode_reason);
    gcs().send_message(MSG_HEARTBEAT);

    notify_mode(control_mode);
    return true;
}