From a0bf57758da039c25261f2ea4beb7975d45fe634 Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Tue, 2 Apr 2024 03:13:32 +0100
Subject: [PATCH] SITL: allow disabling MAVLink  simstate messages

---
 libraries/SITL/SITL.cpp | 10 ++++++++++
 libraries/SITL/SITL.h   |  5 +++++
 2 files changed, 15 insertions(+)

diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp
index abd099baf18099..cdbeb8405d7ecb 100644
--- a/libraries/SITL/SITL.cpp
+++ b/libraries/SITL/SITL.cpp
@@ -1195,6 +1195,11 @@ const Location post_origin {
 /* report SITL state via MAVLink SIMSTATE*/
 void SIM::simstate_send(mavlink_channel_t chan) const
 {
+    if (stop_MAVLink_sim_state) {
+        // Sim only MAVLink messages disabled to give more relaistic data rates
+        return;
+    }
+
     float yaw;
 
     // convert to same conventions as DCM
@@ -1220,6 +1225,11 @@ void SIM::simstate_send(mavlink_channel_t chan) const
 /* report SITL state via MAVLink SIM_STATE */
 void SIM::sim_state_send(mavlink_channel_t chan) const
 {
+    if (stop_MAVLink_sim_state) {
+        // Sim only MAVLink messages disabled to give more relaistic data rates
+        return;
+    }
+
     // convert to same conventions as DCM
     float yaw = state.yawDeg;
     if (yaw > 180) {
diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h
index 5198ba1f32b323..779c763ada9eeb 100644
--- a/libraries/SITL/SITL.h
+++ b/libraries/SITL/SITL.h
@@ -543,6 +543,11 @@ class SIM {
     AP_Int16 osd_rows;
     AP_Int16 osd_columns;
 #endif
+
+    // Allow inhibiting of SITL only sim state messages over MAVLink
+    // This gives more realistic data rates for testing links
+    void set_stop_MAVLink_sim_state() { stop_MAVLink_sim_state = true; }
+    bool stop_MAVLink_sim_state;
 };
 
 } // namespace SITL