PX4 如何切换到offboard 模式

最近研究px4的offboard模式，如何切换到offboard模式一筹莫展，便跟踪源代码至commander module。

一、 handle_command函数

  从dronekit发送设定offboard模式的mavlink信息至飞控，相关命令经mavlink模块解析得到关于vehicle_command的主题。

def PX4setMode(mavMode): vehicle._master.mav.command_long_send(vehicle._master.target_system, vehicle._master.target_component, mavutil.mavlink.MAV_CMD_DO_SET_MODE, 0, 1, mavMode, 0, 0, 0, 0, 0)

由handle_command函数中

                                } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) {
/* OFFBOARD */
main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_OFFBOARD, main_state_prev, &status_flags, &internal_state);
}

相关信息要跟踪至main_state_transition。

二、main_state_transition

   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)
{
transition_result_t ret = TRANSITION_DENIED;


/* transition may be denied even if the same state is requested because conditions may have changed */
switch (new_main_state) {

case commander_state_s::MAIN_STATE_OFFBOARD:


/* need offboard signal */
if (!status_flags->offboard_control_signal_lost) {
ret = TRANSITION_CHANGED;
}


break;

     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;
}
}


return ret;

从以上可知，status_flags->offboard_control_signal_lost 是一个重要的标志位，为0才能使ret = TRANSITION_CHANGED，才能进而可以使main_state_prev = internal_state->main_state;internal_state->main_state = new_main_state。

三、继续跟踪set_nav_state函数

switch (internal_state->main_state)

case commander_state_s::MAIN_STATE_OFFBOARD:


/* require offboard control, otherwise stay where you are */
if (status_flags->offboard_control_signal_lost && !status->rc_signal_lost) {
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc);


if (status_flags->offboard_control_loss_timeout && offb_loss_rc_act < 5 && offb_loss_rc_act >= 0) {
if (offb_loss_rc_act == 3 && status_flags->condition_global_position_valid
   && status_flags->condition_home_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;


} else if (offb_loss_rc_act == 0 && status_flags->condition_global_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;


} else if (offb_loss_rc_act == 1 && status_flags->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;


} else if (offb_loss_rc_act == 2) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;


} else if (offb_loss_rc_act == 4 && status_flags->condition_global_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;


} else if (status_flags->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;


} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}


} else {
if (status_flags->condition_global_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;


} else if (status_flags->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;


} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
}


} else if (status_flags->offboard_control_signal_lost && status->rc_signal_lost) {
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc);


if (status_flags->offboard_control_loss_timeout && offb_loss_act < 3 && offb_loss_act >= 0) {
if (offb_loss_act == 2 && status_flags->condition_global_position_valid
   && status_flags->condition_home_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;


} else if (offb_loss_act == 1 && status_flags->condition_global_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;


} else if (offb_loss_act == 0 && status_flags->condition_global_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;


} else if (status_flags->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;


} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}


} else {
if (status_flags->condition_global_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;


} else if (status_flags->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;


} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
}


} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_OFFBOARD;
}


default:
break;
}


return status->nav_state != nav_state_old;
}

要根据不同的status_flags切换到不同的控制模式。最重要的是使status_flags->offboard_control_signal_lost 为0.

 

四、跟踪status_flags->offboard_control_signal_lost

orb_check(offboard_control_mode_sub, &updated);


if (updated) {
orb_copy(ORB_ID(offboard_control_mode), offboard_control_mode_sub, &offboard_control_mode);
}


if (offboard_control_mode.timestamp != 0 &&
   offboard_control_mode.timestamp + OFFBOARD_TIMEOUT > hrt_absolute_time()) {
if (status_flags.offboard_control_signal_lost) {
status_flags.offboard_control_signal_lost = false;
status_flags.offboard_control_loss_timeout = false;
status_changed = true;
}


} else {
if (!status_flags.offboard_control_signal_lost) {
status_flags.offboard_control_signal_lost = true;
status_changed = true;
}


/* check timer if offboard was there but now lost */
if (!status_flags.offboard_control_loss_timeout && offboard_control_mode.timestamp != 0) {
if (offboard_loss_timeout < FLT_EPSILON) {
/* execute loss action immediately */
status_flags.offboard_control_loss_timeout = true;


} else {
/* wait for timeout if set */
status_flags.offboard_control_loss_timeout = offboard_control_mode.timestamp +
OFFBOARD_TIMEOUT + offboard_loss_timeout * 1e6f < hrt_absolute_time();
}


if (status_flags.offboard_control_loss_timeout) {
status_changed = true;
}
}
}

可发现status_flags.offboard_control_signal_lost与 offboard_control_mode.timestamp 及OFFBOARD_TIMEOUT相关.OFFBOARD_TIMEOUT为固定值，0.5s。继续跟踪offboard_control_mode.timestamp至mavlink_receiver.cpp中。

在MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t *msg)中可发现

offboard_control_mode.timestamp = hrt_absolute_time();

从这些信息可以发现如果要切换到offboard 模式，msgid为SET_POSITION_TARGET_LOCAL_NED的mavlink信息发送频率要大于2HZ，即至少0.5s要发送一次。