/* handle a MISSION_WRITE_PARTIAL_LIST mavlink packet */ void GCS_MAVLINK::handle_mission_write_partial_list(AP_Mission &mission, mavlink_message_t *msg) { // decode mavlink_mission_write_partial_list_t packet; mavlink_msg_mission_write_partial_list_decode(msg, &packet); // exit immediately if this command is not meant for this vehicle if (mavlink_check_target(packet.target_system,packet.target_component)) { return; } // start waypoint receiving if ((unsigned)packet.start_index > mission.num_commands() || (unsigned)packet.end_index > mission.num_commands() || packet.end_index < packet.start_index) { send_text_P(SEVERITY_LOW,PSTR("flight plan update rejected")); return; } waypoint_timelast_receive = hal.scheduler->millis(); waypoint_timelast_request = 0; waypoint_receiving = true; waypoint_request_i = packet.start_index; waypoint_request_last= packet.end_index; }
/* handle a MISSION_WRITE_PARTIAL_LIST mavlink packet */ void GCS_MAVLINK::handle_mission_write_partial_list(AP_Mission &mission, mavlink_message_t *msg) { // decode mavlink_mission_write_partial_list_t packet; mavlink_msg_mission_write_partial_list_decode(msg, &packet); // start waypoint receiving if ((unsigned)packet.start_index > mission.num_commands() || (unsigned)packet.end_index > mission.num_commands() || packet.end_index < packet.start_index) { send_text_P(SEVERITY_LOW,PSTR("flight plan update rejected")); return; } waypoint_timelast_receive = hal.scheduler->millis(); waypoint_timelast_request = 0; waypoint_receiving = true; waypoint_request_i = packet.start_index; waypoint_request_last= packet.end_index; }
/* handle a MISSION_REQUEST_LIST mavlink packet */ void GCS_MAVLINK::handle_mission_request_list(AP_Mission &mission, mavlink_message_t *msg) { // decode mavlink_mission_request_list_t packet; mavlink_msg_mission_request_list_decode(msg, &packet); // reply with number of commands in the mission. The GCS will then request each command separately mavlink_msg_mission_count_send(chan,msg->sysid, msg->compid, mission.num_commands()); // set variables to help handle the expected sending of commands to the GCS waypoint_receiving = false; // record that we are sending commands (i.e. not receiving) waypoint_dest_sysid = msg->sysid; // record system id of GCS who has requested the commands waypoint_dest_compid = msg->compid; // record component id of GCS who has requested the commands }
/* handle an incoming mission item */ void GCS_MAVLINK::handle_mission_item(mavlink_message_t *msg, AP_Mission &mission) { mavlink_mission_item_t packet; uint8_t result = MAV_MISSION_ACCEPTED; struct AP_Mission::Mission_Command cmd = {}; mavlink_msg_mission_item_decode(msg, &packet); if (mavlink_check_target(packet.target_system,packet.target_component)) { return; } // convert mavlink packet to mission command if (!AP_Mission::mavlink_to_mission_cmd(packet, cmd)) { result = MAV_MISSION_INVALID; goto mission_ack; } if (packet.current == 2) { // current = 2 is a flag to tell us this is a "guided mode" // waypoint and not for the mission handle_guided_request(cmd); // verify we received the command result = 0; goto mission_ack; } if (packet.current == 3) { //current = 3 is a flag to tell us this is a alt change only // add home alt if needed handle_change_alt_request(cmd); // verify we recevied the command result = 0; goto mission_ack; } // Check if receiving waypoints (mission upload expected) if (!waypoint_receiving) { result = MAV_MISSION_ERROR; goto mission_ack; } // check if this is the requested waypoint if (packet.seq != waypoint_request_i) { result = MAV_MISSION_INVALID_SEQUENCE; goto mission_ack; } // if command index is within the existing list, replace the command if (packet.seq < mission.num_commands()) { if (mission.replace_cmd(packet.seq,cmd)) { result = MAV_MISSION_ACCEPTED; }else{ result = MAV_MISSION_ERROR; goto mission_ack; } // if command is at the end of command list, add the command } else if (packet.seq == mission.num_commands()) { if (mission.add_cmd(cmd)) { result = MAV_MISSION_ACCEPTED; }else{ result = MAV_MISSION_ERROR; goto mission_ack; } // if beyond the end of the command list, return an error } else { result = MAV_MISSION_ERROR; goto mission_ack; } // update waypoint receiving state machine waypoint_timelast_receive = hal.scheduler->millis(); waypoint_request_i++; if (waypoint_request_i >= waypoint_request_last) { mavlink_msg_mission_ack_send_buf( msg, chan, msg->sysid, msg->compid, MAV_MISSION_ACCEPTED); send_text_P(SEVERITY_LOW,PSTR("flight plan received")); waypoint_receiving = false; // XXX ignores waypoint radius for individual waypoints, can // only set WP_RADIUS parameter } else { waypoint_timelast_request = hal.scheduler->millis(); // if we have enough space, then send the next WP immediately if (comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES >= MAVLINK_MSG_ID_MISSION_ITEM_LEN) { queued_waypoint_send(); } else { send_message(MSG_NEXT_WAYPOINT); } } return; mission_ack: // we are rejecting the mission/waypoint mavlink_msg_mission_ack_send_buf( msg, chan, msg->sysid, msg->compid, result); }