一天精通无人机第 29 讲 中级篇系列:命令响应与状态切换(下)

file

在本节我们来学习Commander中的3种状态切换过程,其中包括无人机的解锁状态处理、主状态处理和导航状态处理。这3种状态是无人机中非常重要的状态,直接影响飞机的控制导航逻辑和控制逻辑,进而影响飞机的实际飞行效果。下面我们就来分别对这3种飞行状态的处理过程进行分析:

一、arming_state解锁/锁定状态

解锁状态在PX4飞控程序中的定义名称为Arming state表示飞机当前的解锁和锁定状态,它被定义成vehicle_status.msg这个uORB中的uint8 arming_state属性,实际上arming_state并不仅仅用于表示“锁定”和“解锁”这两个状态,它一共有6个状态也定义在vehicle_status.msg这个UORB当中,其内容如下:

uint8 ARMING_STATE_INIT = 0             //初始化
uint8 ARMING_STATE_STANDBY = 1          //准备就绪
uint8 ARMING_STATE_ARMED = 2            //已解锁
uint8 ARMING_STATE_STANDBY_ERROR = 3    //就绪错误
uint8 ARMING_STATE_REBOOT = 4           //重启
uint8 ARMING_STATE_IN_AIR_RESTORE = 5   //空中恢复

实际上,Arming state是为了表示无人机在整个飞行过程中可能出现的基本状态,其中只有Arming state为ARMING_STATE_ARMED时,才表示飞控解锁,也就是电机解锁。用于表示飞控实际对电机“锁定”和“解锁”状态的变量并非Arming state,而是actuator_armed.msg这个uORB的bool armed属性。注意这个变量是一个布尔型变量,它只表示无人机系统“解锁”和“锁定”这两个状态。

上述的6个状态切换图如下:

file

在PX4程序中使用了一个二位数组来表示此状态机的切换关系,如下:

static const bool arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle_status_s::ARMING_STATE_MAX] = 
{
    //                                 INIT,  STANDBY, ARMED, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE
    { /*ARMING_STATE_INIT */           true,  true,    false, true,          false,  false },
    { /*ARMING_STATE_STANDBY */        true,  true,    true,  false,         false,  false },
    { /*ARMING_STATE_ARMED */          false, true,    true,  false,         false,  true  },
    { /*ARMING_STATE_STANDBY_ERROR */  true,  true,    true,  true,          false,  false },
    { /*ARMING_STATE_REBOOT */         true,  true,    false, true,          true,   true  },
    { /*ARMING_STATE_IN_AIR_RESTORE */ false, false,   false, false,         false,  false }
};

在这个表示Arming state的二位数组中,每一行表示无人机需要切换的新的状态,每一列表示当前状态。从当前状态切换到新状态时的限制条件就是arming_transitions这个二位数组的值,如果为true表示允许切换,如果为false表示不允许切换。

transition_result_t arming_state_transition(vehicle_status_s *status,
                const arming_state_t new_arming_state,
                actuator_armed_s *armed,
                vehicle_status_flags_s *status_flags,
                ...)
{
    //允许切换标识
    transition_result_t ret = TRANSITION_DENIED;
    //当前状态
    arming_state_t current_arming_state = status->arming_state;
    //如果需要切换的新状态与当前状态相同,则不处理
    if (new_arming_state == current_arming_state)
    {
        ret = TRANSITION_NOT_CHANGED;
    }
    else
    {
        //检查状态切换结果
        bool valid_transition = arming_transitions[new_arming_state][status->arming_state];
        //如果允许切换
        if (valid_transition)
        {
            //设定“解锁”或“锁定”状态
            armed->armed = (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED);
            //完成状态切换
            ret = TRANSITION_CHANGED;
            //更新Arming state为新状态
            status->arming_state = new_arming_state;
        }
    }
    return ret;
}

二、main_state主状态

主状态就是我们之前所介绍的无人机的飞行模式。切换主状态就是切换飞行模式,使无人机进入不同的飞行控制逻辑当中。当然,飞控的主状态不可以任意切换,切换到不同的主状态需要不同的前提条件。每一种状态的条件前提我们可以按下表进行判断:

file

用于表示主状态的变量被定义为commander_state.msg这个uORB中的uint8 main_state属性。其实际值与所表示的状态对应关系如下:

uint8 MAIN_STATE_MANUAL = 0                    //手动模式
uint8 MAIN_STATE_ALTCTL = 1                    //高度控制模式
uint8 MAIN_STATE_POSCTL = 2                    //位置控制模式
uint8 MAIN_STATE_AUTO_MISSION = 3              //任务模式
uint8 MAIN_STATE_AUTO_LOITER = 4               //悬停模式
uint8 MAIN_STATE_AUTO_RTL = 5                  //返航模式
uint8 MAIN_STATE_ACRO = 6                      //特技模式
uint8 MAIN_STATE_OFFBOARD = 7                  //离线模式
uint8 MAIN_STATE_STAB = 8                      //增稳模式
uint8 MAIN_STATE_RATTITUDE = 9                 //手动特技模式
uint8 MAIN_STATE_AUTO_TAKEOFF = 10             //起飞模式
uint8 MAIN_STATE_AUTO_LAND = 11                //着陆模式
uint8 MAIN_STATE_AUTO_FOLLOW_TARGET = 12       //目标跟随模式

在主状态切换函数main_state_transition()处理了主状态的切换逻辑,具体代码如下:

transition_result_t main_state_transition(const vehicle_status_s &status,
        const main_state_t new_main_state,
        const vehicle_status_flags_s &status_flags,
        commander_state_s *internal_state)
{
    //切换结果,允许或拒绝
    transition_result_t ret = TRANSITION_DENIED;
    //需要切换的新主状态
    switch (new_main_state)
    {
        case commander_state_s::MAIN_STATE_MANUAL:
        case commander_state_s::MAIN_STATE_STAB:
        case commander_state_s::MAIN_STATE_ACRO:
        case commander_state_s::MAIN_STATE_RATTITUDE:
            ret = TRANSITION_CHANGED;
            break;
        case commander_state_s::MAIN_STATE_ALTCTL:
            if (status_flags.condition_local_altitude_valid
                    || status_flags.condition_global_position_valid)
                ret = TRANSITION_CHANGED;
            break;
        case commander_state_s::MAIN_STATE_POSCTL:
            if (status_flags.condition_local_position_valid
                    || status_flags.condition_global_position_valid)
                ret = TRANSITION_CHANGED;
            break;
        case commander_state_s::MAIN_STATE_AUTO_LOITER:
            if (status_flags.condition_global_position_valid)
                ret = TRANSITION_CHANGED;
            break;
        case commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET:
            if (status.is_rotary_wing)
                ret = TRANSITION_CHANGED;
            break;
        case commander_state_s::MAIN_STATE_AUTO_MISSION:
            if (status_flags.condition_global_position_valid
                    && status_flags.condition_auto_mission_available)
                ret = TRANSITION_CHANGED;
            break;
        case commander_state_s::MAIN_STATE_AUTO_RTL:
            if (status_flags.condition_global_position_valid
                    && status_flags.condition_home_position_valid)
                ret = TRANSITION_CHANGED;
            break;
        case commander_state_s::MAIN_STATE_AUTO_TAKEOFF:
        case commander_state_s::MAIN_STATE_AUTO_LAND:
            if (status_flags.condition_local_position_valid)
                ret = TRANSITION_CHANGED;
            break;
        case commander_state_s::MAIN_STATE_AUTO_PRECLAND:
            if (status_flags.condition_local_position_valid
                    && status_flags.condition_global_position_valid
                    && status.is_rotary_wing)
                ret = TRANSITION_CHANGED;
            break;
        case commander_state_s::MAIN_STATE_OFFBOARD:
            if (!status_flags.offboard_control_signal_lost)
                ret = TRANSITION_CHANGED;
            break;
        case commander_state_s::MAIN_STATE_MAX:
        default:
            break;
    }
    //如果允许切换主状态,则修改主状态的值
    if (ret == TRANSITION_CHANGED)
        if (internal_state->main_state != new_main_state)
            internal_state->main_state = new_main_state;
        else
            ret = TRANSITION_NOT_CHANGED;
    return ret;
}

以上代码中所处理的逻辑与上述表格中的内容一致,不再赘述。

三、导航状态

导航状态被定义在vehicle_status.msg这个uORB的uint8 nav_state属性,其实际值与导航状态的对应关系如下:

uint8 NAVIGATION_STATE_MANUAL = 0                 //手动模式
uint8 NAVIGATION_STATE_ALTCTL = 1                 //高度控制模式
uint8 NAVIGATION_STATE_POSCTL = 2                 //位置控制模式
uint8 NAVIGATION_STATE_AUTO_MISSION = 3           //自动任务模式
uint8 NAVIGATION_STATE_AUTO_LOITER = 4            //自动悬停模式
uint8 NAVIGATION_STATE_AUTO_RTL = 5               //自动返航模式
uint8 NAVIGATION_STATE_AUTO_RCRECOVER = 6         //遥控器恢复
uint8 NAVIGATION_STATE_AUTO_RTGS = 7              //数传信号丢失自动返回地面站
uint8 NAVIGATION_STATE_AUTO_LANDENGFAIL = 8       //自动降落当引擎失效
uint8 NAVIGATION_STATE_AUTO_LANDGPSFAIL = 9       //自动降落当GPS失效
uint8 NAVIGATION_STATE_ACRO = 10                  //特级模式
uint8 NAVIGATION_STATE_UNUSED = 11                //预留
uint8 NAVIGATION_STATE_DESCEND = 12               //下降模式
uint8 NAVIGATION_STATE_TERMINATION = 13           //自毁模式
uint8 NAVIGATION_STATE_OFFBOARD = 14              //离线模式
uint8 NAVIGATION_STATE_STAB = 15                  //增稳模式
uint8 NAVIGATION_STATE_RATTITUDE = 16             //手动特技模式
uint8 NAVIGATION_STATE_AUTO_TAKEOFF = 17          //自动起飞模式
uint8 NAVIGATION_STATE_AUTO_LAND = 18             //自动着陆模式
uint8 NAVIGATION_STATE_AUTO_FOLLOW_TARGET = 19    //目标跟随模式
uint8 NAVIGATION_STATE_AUTO_PRECLAND = 20         //按目标精准着陆

与主状态切换不同,无人机会根据当前状态与不同的条件对导航状态进行降级评估,也就是说如果当前飞行模式的必要条件如果不满足,则会切入到一个较为安全的导航状态。在正常情况下,导航状态值会被保持与主状态一致。

switch (internal_state->main_state)
{
    case commander_state_s::MAIN_STATE_ACRO:
        status->nav_state = vehicle_status_s::NAVIGATION_STATE_ACRO;
        break;
    case commander_state_s::MAIN_STATE_MANUAL:
        status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
        break;
    case commander_state_s::MAIN_STATE_RATTITUDE:
        status->nav_state = vehicle_status_s::NAVIGATION_STATE_RATTITUDE;
        break;
    case commander_state_s::MAIN_STATE_STAB:
        status->nav_state = vehicle_status_s::NAVIGATION_STATE_STAB;
        break;
    case commander_state_s::MAIN_STATE_ALTCTL:
        status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
        break;
    ...
    default:
        status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
        break;
}

但是,在实际飞行中,会遇到很多异常情况,飞控程序会根据预先对相关保护参数的设定值修改导航状态,例如,当数传信号丢失后的处理方法如下:

void set_link_loss_nav_state(vehicle_status_s *status, actuator_armed_s *armed,
        const vehicle_status_flags_s &status_flags,
        commander_state_s *internal_state,
        const link_loss_actions_t link_loss_act,
        uint8_t auto_recovery_nav_state)
{
    //全局位置和起飞点有效时,自动恢复保护
    if (link_loss_act == link_loss_actions_t::AUTO_RECOVER
            && status_flags.condition_global_position_valid
            && status_flags.condition_home_position_valid)
    {
        status->nav_state = auto_recovery_nav_state;
    }
    //全局位置有效时,自动悬停
    else if (link_loss_act == link_loss_actions_t::AUTO_LOITER
            && status_flags.condition_global_position_valid)
    {
        status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;

    }
    //全局位置和起飞点有效时,自动返航
    else if (link_loss_act == link_loss_actions_t::AUTO_RTL
            && status_flags.condition_global_position_valid
            && status_flags.condition_home_position_valid)
    {
        main_state_transition(*status, commander_state_s::MAIN_STATE_AUTO_RTL,
                status_flags, internal_state);
        status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
    }
    //本地位置有效时,自动降落
    else if (link_loss_act == link_loss_actions_t::AUTO_LAND
            && status_flags.condition_local_position_valid)
    {
        status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;

    }
    //自毁
    else if (link_loss_act == link_loss_actions_t::TERMINATE)
    {
        status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
        armed->force_failsafe = true;

    }
    //锁定
    else if (link_loss_act == link_loss_actions_t::LOCKDOWN)
    {
        armed->lockdown = true;
    }
    //全局位置和起飞点有效时,自动返航
    else if (status_flags.condition_global_position_valid
            && status_flags.condition_home_position_valid)
    {
        status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
    }
    //本地位置有效时自动降落
    else if (status_flags.condition_local_position_valid)
    {
        status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
    }
    //高度有效时
    else if (status_flags.condition_local_altitude_valid)
    {
        //多旋翼自动下降
        if (status->is_rotary_wing)
        {
            status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
        }
        //自动降落当GPS失效
        else
        {
            status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
        }
    }
    //自毁
    else
    {
        status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
    }
}

下期预告:《中级篇:外部通信协议Mavlink(上)》

file