|
|
|
@ -1526,6 +1526,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
@@ -1526,6 +1526,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// check for supported coordinate frames
|
|
|
|
|
if (packet.coordinate_frame != MAV_FRAME_GLOBAL_INT && |
|
|
|
|
packet.coordinate_frame != MAV_FRAME_GLOBAL_RELATIVE_ALT_INT && |
|
|
|
|
packet.coordinate_frame != MAV_FRAME_GLOBAL_TERRAIN_ALT_INT) { |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE; |
|
|
|
|
bool vel_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE; |
|
|
|
|
bool acc_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE; |
|
|
|
@ -1545,17 +1552,14 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
@@ -1545,17 +1552,14 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|
|
|
|
loc.lng = packet.lon_int; |
|
|
|
|
loc.alt = packet.alt*100; |
|
|
|
|
switch (packet.coordinate_frame) { |
|
|
|
|
case MAV_FRAME_GLOBAL_RELATIVE_ALT: |
|
|
|
|
case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT: |
|
|
|
|
loc.flags.relative_alt = true; |
|
|
|
|
loc.flags.terrain_alt = false; |
|
|
|
|
break; |
|
|
|
|
case MAV_FRAME_GLOBAL_TERRAIN_ALT: |
|
|
|
|
case MAV_FRAME_GLOBAL_TERRAIN_ALT_INT: |
|
|
|
|
loc.flags.relative_alt = true; |
|
|
|
|
loc.flags.terrain_alt = true; |
|
|
|
|
break; |
|
|
|
|
case MAV_FRAME_GLOBAL: |
|
|
|
|
case MAV_FRAME_GLOBAL_INT: |
|
|
|
|
default: |
|
|
|
|
loc.flags.relative_alt = false; |
|
|
|
|