From 26b82cf0a3575d181a529529a9de172a14ffcae3 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 28 Feb 2014 15:56:45 +0900 Subject: [PATCH] Rover: use common location options masks --- APMrover2/GCS_Mavlink.pde | 10 +++++----- APMrover2/commands.pde | 6 +++--- APMrover2/defines.h | 4 ---- 3 files changed, 8 insertions(+), 12 deletions(-) diff --git a/APMrover2/GCS_Mavlink.pde b/APMrover2/GCS_Mavlink.pde index ff3501754d..f60e8b41ad 100644 --- a/APMrover2/GCS_Mavlink.pde +++ b/APMrover2/GCS_Mavlink.pde @@ -1273,7 +1273,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // set frame of waypoint uint8_t frame; - if (tell_command.options & MASK_OPTIONS_RELATIVE_ALT) { + if (tell_command.options & LOCATION_MASK_OPTIONS_RELATIVE_ALT) { frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; // reference frame } else { frame = MAV_FRAME_GLOBAL; // reference frame @@ -1560,7 +1560,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) (radius_of_earth*cosf(ToRad(home.lat/1.0e7f)))) + home.lat; tell_command.lng = 1.0e7f*ToDeg(packet.y/radius_of_earth) + home.lng; tell_command.alt = -packet.z*1.0e2f; - tell_command.options = MASK_OPTIONS_RELATIVE_ALT; + tell_command.options = LOCATION_MASK_OPTIONS_RELATIVE_ALT; break; } #endif @@ -1572,7 +1572,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) (radius_of_earth*cosf(ToRad(home.lat/1.0e7f)))) + home.lat; tell_command.lng = 1.0e7f*ToDeg(packet.y/radius_of_earth) + home.lng; tell_command.alt = packet.z*1.0e2f; - tell_command.options = MASK_OPTIONS_RELATIVE_ALT; + tell_command.options = LOCATION_MASK_OPTIONS_RELATIVE_ALT; break; } #endif @@ -1582,7 +1582,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) tell_command.lat = 1.0e7f * packet.x; // in as DD converted to * t7 tell_command.lng = 1.0e7f * packet.y; // in as DD converted to * t7 tell_command.alt = packet.z * 1.0e2f; - tell_command.options = MASK_OPTIONS_RELATIVE_ALT; // store altitude relative!! Always!! + tell_command.options = LOCATION_MASK_OPTIONS_RELATIVE_ALT; // store altitude relative!! Always!! break; } @@ -1659,7 +1659,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) guided_WP = tell_command; // add home alt if needed - if (guided_WP.options & MASK_OPTIONS_RELATIVE_ALT){ + if (guided_WP.options & LOCATION_MASK_OPTIONS_RELATIVE_ALT){ guided_WP.alt += home.alt; } diff --git a/APMrover2/commands.pde b/APMrover2/commands.pde index e4952514ad..652ad70efa 100644 --- a/APMrover2/commands.pde +++ b/APMrover2/commands.pde @@ -56,7 +56,7 @@ static struct Location get_cmd_with_index(int i) } // Add on home altitude if we are a nav command (or other command with altitude) and stored alt is relative - if((temp.id < MAV_CMD_NAV_LAST || temp.id == MAV_CMD_CONDITION_CHANGE_ALT) && temp.options & MASK_OPTIONS_RELATIVE_ALT){ + if((temp.id < MAV_CMD_NAV_LAST || temp.id == MAV_CMD_CONDITION_CHANGE_ALT) && temp.options & LOCATION_MASK_OPTIONS_RELATIVE_ALT){ temp.alt += home.alt; } @@ -72,8 +72,8 @@ static void set_cmd_with_index(struct Location temp, int i) // Set altitude options bitmask // XXX What is this trying to do? - if ((temp.options & MASK_OPTIONS_RELATIVE_ALT) && i != 0){ - temp.options = MASK_OPTIONS_RELATIVE_ALT; + if ((temp.options & LOCATION_MASK_OPTIONS_RELATIVE_ALT) && i != 0){ + temp.options = LOCATION_MASK_OPTIONS_RELATIVE_ALT; } else { temp.options = 0; } diff --git a/APMrover2/defines.h b/APMrover2/defines.h index 9bcd2bc4ec..77a213aa02 100644 --- a/APMrover2/defines.h +++ b/APMrover2/defines.h @@ -66,10 +66,6 @@ enum mode { #define NO_COMMAND 0 #define WAIT_COMMAND 255 -// Command/Waypoint/Location Options Bitmask -//-------------------- -#define MASK_OPTIONS_RELATIVE_ALT (1<<0) // 1 = Relative altitude - //repeating events #define NO_REPEAT 0 #define CH_5_TOGGLE 1