diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index c7860ee6bf..ef851980e0 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -873,6 +873,11 @@ bool GCS_MAVLINK_Plane::try_send_message(enum ap_message id) CHECK_PAYLOAD_SIZE(MAG_CAL_REPORT); plane.compass.send_mag_cal_report(chan); break; + + case MSG_ADSB_VEHICLE: + CHECK_PAYLOAD_SIZE(ADSB_VEHICLE); + plane.adsb.send_adsb_vehicle(chan); + break; } return true; } @@ -962,6 +967,15 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = { // @Increment: 1 // @User: Advanced AP_GROUPINFO("PARAMS", 8, GCS_MAVLINK, streamRates[8], 10), + + // @Param: ADSB + // @DisplayName: ADSB stream rate to ground station + // @Description: ADSB stream rate to ground station + // @Units: Hz + // @Range: 0 50 + // @Increment: 1 + // @User: Advanced + AP_GROUPINFO("ADSB", 9, GCS_MAVLINK, streamRates[9], 5), AP_GROUPEND }; @@ -1081,6 +1095,12 @@ GCS_MAVLINK_Plane::data_stream_send(void) send_message(MSG_GIMBAL_REPORT); send_message(MSG_VIBRATION); } + + if (plane.gcs_out_of_time) return; + + if (stream_trigger(STREAM_ADSB)) { + send_message(MSG_ADSB_VEHICLE); + } }