diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index 92fb259556..4e2407baad 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -560,6 +560,11 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id) send_location(chan); break; + case MSG_LOCAL_POSITION: + CHECK_PAYLOAD_SIZE(LOCAL_POSITION_NED); + send_local_position(ahrs); + break; + case MSG_NAV_CONTROLLER_OUTPUT: if (control_mode != MANUAL) { CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT); @@ -901,6 +906,7 @@ GCS_MAVLINK::data_stream_send(void) if (stream_trigger(STREAM_POSITION)) { // sent with GPS read send_message(MSG_LOCATION); + send_message(MSG_LOCAL_POSITION); } if (gcs_out_of_time) return;