#41 的分析:
- 通过此句检测到上传的目标序列点。
case MAVLINK_MSG_ID_MISSION_SET_CURRENT:
handle_common_mission_message(msg);
break;
- 再次switch循环。
case MAVLINK_MSG_ID_MISSION_SET_CURRENT: // MAV ID: 41
{
handle_mission_set_current(*_mission, msg);
break;
}
handle_mission_set_current() 函数
/*
handle a MISSION_SET_CURRENT mavlink packet
*/
void GCS_MAVLINK::handle_mission_set_current(AP_Mission &mission, mavlink_message_t *msg)
{
// decode
mavlink_mission_set_current_t packet;
mavlink_msg_mission_set_current_decode(msg, &packet);
// set current command
if (mission.set_current_cmd(packet.seq)) {
mavlink_msg_mission_current_send(chan, packet.seq);
}
}
- 解算出当前点,并设置此点。
备注: 若是index = 0,则设置航点完成并且重新开始执行航点。
如果任务停止或完成,将导航命令索引移动到指定点,并将状态设置为停止以至于如果用户继续执行任务,它将从指定的索引开始。
// set_current_cmd - jumps to command specified by index
bool AP_Mission::set_current_cmd(uint16_t index)
{
Mission_Command cmd;
// sanity check index and that we have a mission
if (index >= (unsigned)_cmd_total || _cmd_total == 1) {
return false;
}
// stop the current running do command
_do_cmd.index = AP_MISSION_CMD_INDEX_NONE;
_flags.do_cmd_loaded = false;
_flags.do_cmd_all_done = false;
// stop current nav cmd
_flags.nav_cmd_loaded = false;
// if index is zero then the user wants to completely restart the mission
if (index == 0 || _flags.state == MISSION_COMPLETE) {
_prev_nav_cmd_id = AP_MISSION_CMD_ID_NONE;
_prev_nav_cmd_index = AP_MISSION_CMD_INDEX_NONE;
_prev_nav_cmd_wp_index = AP_MISSION_CMD_INDEX_NONE;
// reset the jump tracking to zero
init_jump_tracking();
if (index == 0) {
index = 1;
}
}
// if the mission is stopped or completed move the nav_cmd index to the specified point and set the state to stopped
// so that if the user resumes the mission it will begin at the specified index
if (_flags.state != MISSION_RUNNING) {
// search until we find next nav command or reach end of command list
while (!_flags.nav_cmd_loaded) {
// get next command
if (!get_next_cmd(index, cmd, true)) {
_nav_cmd.index = AP_MISSION_CMD_INDEX_NONE;
return false;
}
// check if navigation or "do" command
if (is_nav_cmd(cmd)) {
// set current navigation command
_nav_cmd = cmd;
_flags.nav_cmd_loaded = true;
}else{
// set current do command
if (!_flags.do_cmd_loaded) {
_do_cmd = cmd;
_flags.do_cmd_loaded = true;
}
}
// move onto next command
index = cmd.index+1;
}
// if we have not found a do command then set flag to show there are no do-commands to be run before nav command completes
if (!_flags.do_cmd_loaded) {
_flags.do_cmd_all_done = true;
}
// if we got this far then the mission can safely be "resumed" from the specified index so we set the state to "stopped"
_flags.state = MISSION_STOPPED;
return true;
}
// the state must be MISSION_RUNNING
// search until we find next nav command or reach end of command list
while (!_flags.nav_cmd_loaded) {
// get next command
if (!get_next_cmd(index, cmd, true)) {
// if we run out of nav commands mark mission as complete
complete();
// return true because we did what was requested
// which was apparently to jump to a command at the end of the mission
return true;
}
// check if navigation or "do" command
if (is_nav_cmd(cmd)) {
// save previous nav command index
_prev_nav_cmd_id = _nav_cmd.id;
_prev_nav_cmd_index = _nav_cmd.index;
// save separate previous nav command index if it contains lat,long,alt
if (!(cmd.content.location.lat == 0 && cmd.content.location.lng == 0)) {
_prev_nav_cmd_wp_index = _nav_cmd.index;
}
// set current navigation command and start it
_nav_cmd = cmd;
_flags.nav_cmd_loaded = true;
_cmd_start_fn(_nav_cmd);
}else{
// set current do command and start it (if not already set)
if (!_flags.do_cmd_loaded) {
_do_cmd = cmd;
_flags.do_cmd_loaded = true;
_cmd_start_fn(_do_cmd);
}
}
// move onto next command
index = cmd.index+1;
}
// if we have not found a do command then set flag to show there are no do-commands to be run before nav command completes
if (!_flags.do_cmd_loaded) {
_flags.do_cmd_all_done = true;
}
// if we got this far we must have successfully advanced the nav command
return true;
}