From e4596efe9becf0ae6c92825bd0d8888eede3826a Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 3 Jun 2020 14:34:23 +1000
Subject: [PATCH] SITL: added SIM_RATE_HZ

this allows the physics step size to be changed while flying the
internal models, which allows for lower CPU usage
---
 libraries/SITL/SIM_Aircraft.cpp | 3 +++
 libraries/SITL/SITL.cpp         | 2 ++
 libraries/SITL/SITL.h           | 1 +
 3 files changed, 6 insertions(+)

diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp
index e235d5c993..e64c4cc46a 100644
--- a/libraries/SITL/SIM_Aircraft.cpp
+++ b/libraries/SITL/SIM_Aircraft.cpp
@@ -598,6 +598,9 @@ void Aircraft::update_dynamics(const Vector3f &rot_accel)
         }
         }
     }
+
+    // allow for changes in physics step
+    adjust_frame_time(constrain_float(sitl->loop_rate_hz, rate_hz-1, rate_hz+1));
 }
 
 /*
diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp
index 5dab4970a8..8ea7a75a8c 100644
--- a/libraries/SITL/SITL.cpp
+++ b/libraries/SITL/SITL.cpp
@@ -257,6 +257,8 @@ const AP_Param::GroupInfo SITL::var_info3[] = {
     // vicon velocity glitch in NED frame
     AP_GROUPINFO("VICON_VGLI",    21, SITL,  vicon_vel_glitch, 0),
 
+    AP_GROUPINFO("RATE_HZ",  22, SITL,  loop_rate_hz, 1200),
+
     AP_GROUPEND
 
 };
diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h
index 87736902d0..ff530fd02c 100644
--- a/libraries/SITL/SITL.h
+++ b/libraries/SITL/SITL.h
@@ -194,6 +194,7 @@ public:
     AP_Float mag_scaling; // scaling factor on first compasses
     AP_Int32 mag_devid[MAX_CONNECTED_MAGS]; // Mag devid
     AP_Float buoyancy; // submarine buoyancy in Newtons
+    AP_Int16 loop_rate_hz;
 
     // EFI type
     enum EFIType {