diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index 4300772a46..5878b6e6b1 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -1447,7 +1447,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // 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, false)) { + if (!AP_Mission::mission_cmd_to_mavlink(cmd, ret_packet, true)) { goto mission_item_send_failed; } @@ -1688,7 +1688,7 @@ mission_item_send_failed: 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, false)) { + if (!AP_Mission::mavlink_to_mission_cmd(packet, cmd, true)) { result = MAV_MISSION_ERROR; goto mission_failed; } diff --git a/ArduPlane/commands.pde b/ArduPlane/commands.pde index 4baa3c4b6b..f43e40c365 100644 --- a/ArduPlane/commands.pde +++ b/ArduPlane/commands.pde @@ -35,9 +35,15 @@ static void set_next_WP(const AP_Mission::Mission_Command& cmd) // additionally treat zero altitude as current altitude if (next_WP.content.location.alt == 0) { next_WP.content.location.alt = current_loc.alt; + next_WP.content.location.flags.relative_alt = false; } } + // convert relative alt to absolute alt + if (next_WP.content.location.flags.relative_alt) { + next_WP.content.location.flags.relative_alt = false; + next_WP.content.location.alt += home.alt; + } // are we already past the waypoint? This happens when we jump // waypoints, and it can cause us to skip a waypoint. If we are