Pixhawk原生固件PX4之commander函数

欢迎交流~ 个人 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**模式

                                          By Fantasy