diff --git a/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp b/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp index a5e607b808..5855188dc0 100644 --- a/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp +++ b/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp @@ -8,6 +8,7 @@ #include "SoloGimbal.h" #include #include +#include extern const AP_HAL::HAL& hal; @@ -107,6 +108,9 @@ void AP_Mount_SoloGimbal::status_msg(mavlink_channel_t chan) if (_gimbal.aligned()) { mavlink_msg_mount_status_send(chan, 0, 0, degrees(_angle_ef_target_rad.y)*100, degrees(_angle_ef_target_rad.x)*100, degrees(_angle_ef_target_rad.z)*100); } + + // block heartbeat from transmitting to the GCS + GCS_MAVLINK::disable_channel_routing(chan); } /*