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