Pixhawk原生固件PX4之commander函数

最新推荐文章于 2023-12-21 23:25:53 发布

FantasyJXF

最新推荐文章于 2023-12-21 23:25:53 发布

阅读量2.9k

点赞数 2

分类专栏： Pix学习笔记文章标签： Pixhawk PX4 commander 函数

本文链接：https://blog.csdn.net/oqqENvY12/article/details/54948360

版权

Pix学习笔记专栏收录该内容

34 篇文章 49 订阅

订阅专栏

欢迎交流~ 个人 Gitter 交流平台，点击直达：

commaner.cpp包含了PX4中状态机的逻辑，特此对此段代码中的部分关键函数进行解析。

set_main_state_rc

transition_result_t
set_main_state_rc(struct vehicle_status_s *status_local)

说明：根据遥控器开关设置主状态。

参数：

status_local：飞行器应该进入的导航状态

返回值：

res = main_state_transition(…);

功能：使能刹车的使用，根据用户输入的目标状态，运用降级控制策略：为用户提供所需的最接近的飞行模式。分为四个部分MANUAL、ASSIST、AUTO、OFFBOARD，依次进行评估。

main_state_transition

transition_result_t
main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state, uint8_t &main_state_prev,
              status_flags_s *status_flags, struct commander_state_s *internal_state)

说明：飞行模式切换的条件判断函数

参数：

status：导航状态，飞行器应该进入的状态；

new_main_state：期望切换到的新状态

&main_state_prev：之前的状态

status_flags： commander内部的状态标志；

internal_state：主状态，用户想要的状态。由遥控器设置，也可以由地面站通过数传控制

返回值：

ret = TRANSITION_DENIED; // 表示不满足切换条件，拒绝状态切换

ret = TRANSITION_CHANGED; // 切换到某个状态，不一定是用户想要的目标状态，会根据降级策略，切换至一个最接近的状态。

ret = TRANSITION_NOT_CHANGED;

if (ret == TRANSITION_CHANGED) {
        if (internal_state->main_state != new_main_state) {
            main_state_prev = internal_state->main_state;
            internal_state->main_state = new_main_state;  // 更新状态
            internal_state->timestamp = hrt_absolute_time();

        } else {
            ret = TRANSITION_NOT_CHANGED;
        }
    }

功能：对用户输入的new_main_state进行条件判断，切换到一个满足飞行器实际状况的最接近的飞行模式。

set_nav_state

bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *internal_state,
           const bool data_link_loss_enabled, const bool mission_finished,
           const bool stay_in_failsafe, status_flags_s *status_flags, bool landed,
           const bool rc_loss_enabled, const int offb_loss_act, const int offb_loss_rc_act)
{
  navigation_state_t nav_state_old = status->nav_state;
  ...
  ...
  return status->nav_state != nav_state_old;
}

说明：检查失效保护和主状态，然后根据具体情况为navigator设置导航状态

参数：

status：导航状态，飞行器应该进入的状态；

internal_state：主状态，用户想要的状态；

data_link_loss_enabled：与地面站的数据链丢失；

mission_finished：任务完成；

stay_in_failsafe：故障保护；

status_flags：commander内部的状态标志；

landed：着陆；

rc_loss_enabled：遥控信号丢失；

offb_loss_act：

offb_loss_rc_act：

返回值：

1; // 导航状态改变

0; // 未改变

功能：根据commander内部的internal_state主状态，在飞机出现故障时进行失效保护状态切换，评估飞行器应该进入何种飞行模式。

一般情况为：

①global位置有效 && 起飞点(home)有效 && GPS有效

进入NAVIGATION_STATE_AUTO_**RCRECOVER**模式

②local位置有效 && GPS有效

进入NAVIGATION_STATE_AUTO_**LAND**模式(自动降落)

③local高度有效

进入NAVIGATION_STATE_**DESCEND**模式(无位置控制的下降模式)

④终止飞行

进入NAVIGATION_STATE_**TERMINATION**模式