最近研究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要发送一次。
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)
