diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index e21c712b32..0e6e9482a6 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -1165,26 +1165,17 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) if (!check_latlng(packet.lat_int, packet.lon_int)) { break; } - Location loc; - loc.lat = packet.lat_int; - loc.lng = packet.lon_int; - loc.alt = packet.alt*100; - switch (packet.coordinate_frame) { - case MAV_FRAME_GLOBAL_RELATIVE_ALT: // solo shot manager incorrectly sends RELATIVE_ALT instead of RELATIVE_ALT_INT - case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT: - loc.relative_alt = true; - loc.terrain_alt = false; - break; - case MAV_FRAME_GLOBAL_TERRAIN_ALT: - case MAV_FRAME_GLOBAL_TERRAIN_ALT_INT: - loc.relative_alt = true; - loc.terrain_alt = true; - break; - case MAV_FRAME_GLOBAL: - case MAV_FRAME_GLOBAL_INT: - default: - break; + Location::ALT_FRAME frame; + if (!mavlink_coordinate_frame_to_location_alt_frame(packet.coordinate_frame, frame)) { + // unknown coordinate frame + break; } + const Location loc{ + packet.lat_int, + packet.lon_int, + int32_t(packet.alt*100), + frame, + }; if (!loc.get_vector_from_origin_NEU(pos_neu_cm)) { break; }