Plane: use common MISSION_ITEM code

This commit is contained in:
Andrew Tridgell 2014-03-19 09:59:51 +11:00 committed by Randy Mackay
parent 8d89a64312
commit b74fddade3

View File

@ -1126,10 +1126,42 @@ GCS_MAVLINK::send_message(enum ap_message id)
mavlink_send_message(chan, id);
}
/*
handle a request to switch to guided mode. This happens via a
callback from handle_mission_item()
*/
void GCS_MAVLINK::handle_guided_request(AP_Mission::Mission_Command &cmd)
{
guided_WP_loc = cmd.content.location;
// add home alt if needed
if (guided_WP_loc.flags.relative_alt) {
guided_WP_loc.alt += home.alt;
}
set_mode(GUIDED);
// make any new wp uploaded instant (in case we are already in Guided mode)
set_guided_WP();
}
/*
handle a request to change current WP altitude. This happens via a
callback from handle_mission_item()
*/
void GCS_MAVLINK::handle_change_alt_request(AP_Mission::Mission_Command &cmd)
{
if (cmd.content.location.flags.relative_alt) {
cmd.content.location.alt += home.alt;
}
next_WP_loc.alt = cmd.content.location.alt;
}
void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
{
struct AP_Mission::Mission_Command cmd = {}; // general purpose mission command
switch (msg->msgid) {
case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
@ -1339,7 +1371,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// XXX read a WP from EEPROM and send it to the GCS
case MAVLINK_MSG_ID_MISSION_REQUEST:
{
handle_mission_request(mission, msg, cmd);
handle_mission_request(mission, msg);
break;
}
@ -1408,128 +1440,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// GCS has sent us a command from GCS, store to EEPROM
case MAVLINK_MSG_ID_MISSION_ITEM:
{
// decode
mavlink_mission_item_t packet;
uint8_t result = MAV_MISSION_ACCEPTED;
mavlink_msg_mission_item_decode(msg, &packet);
if (mavlink_check_target(packet.target_system,packet.target_component)) break;
// convert mavlink packet to mission command
if (!AP_Mission::mavlink_to_mission_cmd(packet, cmd)) {
result = MAV_MISSION_ERROR;
goto mission_failed;
}
if(packet.current == 2) { //current = 2 is a flag to tell us this is a "guided mode" waypoint and not for the mission
guided_WP_loc = cmd.content.location;
// add home alt if needed
if (guided_WP_loc.flags.relative_alt) {
guided_WP_loc.alt += home.alt;
}
set_mode(GUIDED);
// make any new wp uploaded instant (in case we are already in Guided mode)
set_guided_WP();
// verify we recevied the command
mavlink_msg_mission_ack_send_buf(
msg,
chan,
msg->sysid,
msg->compid,
0);
} else if(packet.current == 3) { //current = 3 is a flag to tell us this is a alt change only
// add home alt if needed
if (cmd.content.location.flags.relative_alt) {
cmd.content.location.alt += home.alt;
}
next_WP_loc.alt = cmd.content.location.alt;
// verify we recevied the command
mavlink_msg_mission_ack_send_buf(
msg,
chan,
msg->sysid,
msg->compid,
0);
} else {
// Check if receiving waypoints (mission upload expected)
if (!waypoint_receiving) {
result = MAV_MISSION_ERROR;
goto mission_failed;
}
// check if this is the requested waypoint
if (packet.seq != waypoint_request_i) {
result = MAV_MISSION_INVALID_SEQUENCE;
goto mission_failed;
}
// 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_failed;
}
// 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_failed;
}
// if beyond the end of the command list, return an error
}else{
result = MAV_MISSION_ERROR;
goto mission_failed;
}
// update waypoint receiving state machine
waypoint_timelast_receive = 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);
}
}
}
break;
mission_failed:
// we are rejecting the mission/waypoint
mavlink_msg_mission_ack_send_buf(
msg,
chan,
msg->sysid,
msg->compid,
result);
handle_mission_item(msg, mission);
break;
}