Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 16:48:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: use new handle_mission_request() function

Browse Source
This commit is contained in:
Andrew Tridgell 2014-03-18 13:52:27 +11:00 committed by Randy Mackay
parent add2416dbe
commit 740ecc2c1b
1 changed files with 1 additions and 67 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

68
ArduPlane/GCS_Mavlink.pde
View File

@ -1432,73 +1432,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:
{
// decode
mavlink_mission_request_t packet;
mavlink_msg_mission_request_decode(msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component))
break;
// retrieve mission from eeprom
if (!mission.read_cmd_from_storage(packet.seq, cmd)) {
goto mission_item_send_failed;
}
// convert mission command to mavlink mission item packet
mavlink_mission_item_t ret_packet;
memset(&ret_packet, 0, sizeof(ret_packet));
if (!AP_Mission::mission_cmd_to_mavlink(cmd, ret_packet)) {
goto mission_item_send_failed;
}
// set packet's current field to 1 if this is the command being executed
if (cmd.id == (uint16_t)mission.get_current_nav_cmd().index) {
ret_packet.current = 1;
}else{
ret_packet.current = 0;
}
// set auto continue to 1
ret_packet.autocontinue = 1; // 1 (true), 0 (false)
// plane specific overrides to packet values
switch (cmd.id) {
case MAV_CMD_NAV_LOITER_TIME:
case MAV_CMD_NAV_LOITER_TURNS:
if (cmd.content.location.flags.loiter_ccw) {
ret_packet.param3 = -abs(g.loiter_radius);
} else {
ret_packet.param3 = abs(g.loiter_radius);
}
break;
case MAV_CMD_NAV_LOITER_UNLIM:
if (cmd.content.location.flags.loiter_ccw) {
ret_packet.param3 = -abs(g.loiter_radius);
} else {
ret_packet.param3 = abs(g.loiter_radius);
}
break;
}
mavlink_msg_mission_item_send(chan,msg->sysid,
msg->compid,
packet.seq,
ret_packet.frame,
cmd.id,
ret_packet.current,
ret_packet.autocontinue,
ret_packet.param1,
ret_packet.param2,
ret_packet.param3,
ret_packet.param4,
ret_packet.x,
ret_packet.y,
ret_packet.z);
break;
mission_item_send_failed:
// send failure message
mavlink_msg_mission_ack_send(chan, msg->sysid, msg->compid, MAV_MISSION_ERROR);
handle_mission_request(mission, msg, cmd);
break;
}