diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index 87ec744660..d98c03a31a 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -1440,7 +1440,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 @@ -1469,7 +1469,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAV_CMD_NAV_LOITER_TIME: case MAV_CMD_NAV_LOITER_TURNS: - if (tell_command.options & MASK_OPTIONS_LOITER_DIRECTION) { + if (tell_command.options & LOCATION_MASK_OPTIONS_RELATIVE_ALT) { param3 = -abs(g.loiter_radius); } else { param3 = abs(g.loiter_radius); @@ -1480,7 +1480,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; case MAV_CMD_NAV_LOITER_UNLIM: - if (tell_command.options & MASK_OPTIONS_LOITER_DIRECTION) { + if (tell_command.options & LOCATION_MASK_OPTIONS_RELATIVE_ALT) { param3 = -abs(g.loiter_radius); } else { param3 = abs(g.loiter_radius); @@ -1741,7 +1741,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 @@ -1753,7 +1753,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 @@ -1763,7 +1763,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; } @@ -1779,7 +1779,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) switch (tell_command.id) { case MAV_CMD_NAV_LOITER_UNLIM: if (packet.param3 < 0) { - tell_command.options |= MASK_OPTIONS_LOITER_DIRECTION; + tell_command.options |= LOCATION_MASK_OPTIONS_RELATIVE_ALT; } case MAV_CMD_NAV_WAYPOINT: case MAV_CMD_NAV_RETURN_TO_LAUNCH: @@ -1789,7 +1789,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAV_CMD_NAV_LOITER_TURNS: case MAV_CMD_NAV_LOITER_TIME: if (packet.param3 < 0) { - tell_command.options |= MASK_OPTIONS_LOITER_DIRECTION; + tell_command.options |= LOCATION_MASK_OPTIONS_RELATIVE_ALT; } case MAV_CMD_NAV_TAKEOFF: case MAV_CMD_DO_SET_HOME: @@ -1855,7 +1855,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; } @@ -1874,7 +1874,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) } 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 (tell_command.options & MASK_OPTIONS_RELATIVE_ALT) { + if (tell_command.options & LOCATION_MASK_OPTIONS_RELATIVE_ALT) { tell_command.alt += home.alt; } diff --git a/ArduPlane/commands.pde b/ArduPlane/commands.pde index 67f85823db..2a93d10161 100644 --- a/ArduPlane/commands.pde +++ b/ArduPlane/commands.pde @@ -80,7 +80,7 @@ static struct Location get_cmd_with_index(int16_t 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) && + (temp.options & LOCATION_MASK_OPTIONS_RELATIVE_ALT) && (temp.lat != 0 || temp.lng != 0 || temp.alt != 0)) { temp.alt += home.alt; } @@ -97,10 +97,10 @@ static void set_cmd_with_index(struct Location temp, int16_t i) // force home wp to absolute height if (i == 0) { - temp.options &= ~(MASK_OPTIONS_RELATIVE_ALT); + temp.options &= ~(LOCATION_MASK_OPTIONS_RELATIVE_ALT); } // zero unused bits - temp.options &= (MASK_OPTIONS_RELATIVE_ALT | MASK_OPTIONS_LOITER_DIRECTION); + temp.options &= (LOCATION_MASK_OPTIONS_RELATIVE_ALT | LOCATION_MASK_OPTIONS_LOITER_DIRECTION); hal.storage->write_byte(mem, temp.id); diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde index 8374441cde..d89eb85c02 100644 --- a/ArduPlane/commands_logic.pde +++ b/ArduPlane/commands_logic.pde @@ -274,7 +274,7 @@ static void do_land() static void loiter_set_direction_wp(const struct Location *nav_command) { - if (nav_command->options & MASK_OPTIONS_LOITER_DIRECTION) { + if (nav_command->options & LOCATION_MASK_OPTIONS_LOITER_DIRECTION) { loiter.direction = -1; } else { loiter.direction = 1; diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index d56319d083..5d96b62ff0 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -102,13 +102,6 @@ enum ChannelMixing { #define NO_COMMAND 0 #define WAIT_COMMAND 255 -// Command/Waypoint/Location Options Bitmask -//-------------------- -#define MASK_OPTIONS_RELATIVE_ALT (1<<0) // 1 = Relative - // altitude -#define MASK_OPTIONS_LOITER_DIRECTION (1<<2) // 0 = CW - // 1 = CCW - //repeating events #define NO_REPEAT 0 diff --git a/ArduPlane/rally.pde b/ArduPlane/rally.pde index 30ca80d37a..9db6f6d16d 100644 --- a/ArduPlane/rally.pde +++ b/ArduPlane/rally.pde @@ -68,7 +68,7 @@ static Location rally_location_to_location(const RallyLocation &r_loc, const Loc Location ret = {}; ret.id = MAV_CMD_NAV_LOITER_UNLIM; - ret.options = MASK_OPTIONS_RELATIVE_ALT; + ret.options = LOCATION_MASK_OPTIONS_RELATIVE_ALT; //Currently can't do true AGL on the APM. Relative altitudes are //relative to HOME point's altitude. Terrain on the board is inbound