AP_Mission: added flight stage FLIGHT_LAND_ABORT

- add get_prev_nav_cmd_with_wp_index(). This is different than get_prev_nav_cmd_index() in that it only stores the index if there is a valid lat/lng (+1 squashed commits)
- added mission item command to NAV_LAND which is the abort takeoff altitude. If 0 then use last takeoff if available, else use 50m
This commit is contained in:
Tom Pittenger 2015-08-28 18:33:48 -07:00 committed by Andrew Tridgell
parent 46a83c7ae9
commit cb20325593
1 changed files with 2 additions and 0 deletions

View File

@ -528,6 +528,7 @@ bool AP_Mission::mavlink_to_mission_cmd(const mavlink_mission_item_t& packet, AP
case MAV_CMD_NAV_LAND: // MAV ID: 21
copy_location = true;
cmd.p1 = packet.param1; // abort target altitude(m) (plane only)
break;
case MAV_CMD_NAV_TAKEOFF: // MAV ID: 22
@ -859,6 +860,7 @@ bool AP_Mission::mission_cmd_to_mavlink(const AP_Mission::Mission_Command& cmd,
case MAV_CMD_NAV_LAND: // MAV ID: 21
copy_location = true;
packet.param1 = cmd.p1; // abort target altitude(m) (plane only)
break;
case MAV_CMD_NAV_TAKEOFF: // MAV ID: 22