Pixhawk原生固件PX4之commander函数

2019-11-09 15:52:27

字体：大中小

来源：转载

供稿：网友

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

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

set_main_state_rc

transition_result_tset_main_state_rc(struct vehicle_status_s *status_local)

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

参数：

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

返回值：

res = main_state_transition(…);

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

main_state_transition

transition_result_tmain_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：