From a6c3fb83365a26844742e82506e950912ed2525e Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Sat, 11 Jan 2025 12:44:51 +1100
Subject: [PATCH] AP_HAL_SITL: add Volz simulator

---
 libraries/AP_HAL_SITL/SITL_State.cpp        | 15 +++++++++++++++
 libraries/AP_HAL_SITL/SITL_State_common.cpp |  5 +++++
 2 files changed, 20 insertions(+)

diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp
index 96e3516d8f..31510334f4 100644
--- a/libraries/AP_HAL_SITL/SITL_State.cpp
+++ b/libraries/AP_HAL_SITL/SITL_State.cpp
@@ -392,9 +392,24 @@ void SITL_State::_simulator_servos(struct sitl_input &input)
         }
     }
 
+#if AP_SIM_VOLZ_ENABLED
+    // update simulation input based on data received via "serial" to
+    // Volz servos:
+    if (_sitl->volz_sim.enabled()) {
+        _sitl->volz_sim.update_sitl_input_pwm(input);
+        for (uint8_t i=0; i<ARRAY_SIZE(input.servos); i++) {
+            if (input.servos[i] != 0 && input.servos[i] < 1000) {
+                AP_HAL::panic("Bad input servo value (%u)", input.servos[i]);
+            }
+        }
+    }
+#endif
+
     const float engine_mul = _sitl->engine_mul.get();
     const uint32_t engine_fail = _sitl->engine_fail.get();
 
+    float throttle = 0.0f;
+    
     // apply engine multiplier to motor defined by the SIM_ENGINE_FAIL parameter
     for (uint8_t i=0; i<ARRAY_SIZE(input.servos); i++) {
         if (engine_fail & (1<<i)) {
diff --git a/libraries/AP_HAL_SITL/SITL_State_common.cpp b/libraries/AP_HAL_SITL/SITL_State_common.cpp
index b4e6ae1742..fd7e15a29f 100644
--- a/libraries/AP_HAL_SITL/SITL_State_common.cpp
+++ b/libraries/AP_HAL_SITL/SITL_State_common.cpp
@@ -204,6 +204,11 @@ SITL::SerialDevice *SITL_State_Common::create_serial_sim(const char *name, const
         sitl_model->set_ie24(&_sitl->ie24_sim);
         return &_sitl->ie24_sim;
 #endif // HAL_BUILD_AP_PERIPH
+#if AP_SIM_VOLZ_ENABLED
+    } else if (streq(name, "volz")) {
+        sitl_model->set_volz(&_sitl->volz_sim);
+        return &_sitl->volz_sim;
+#endif  // AP_SIM_VOLZ_ENABLED
     } else if (streq(name, "megasquirt")) {
         if (efi_ms != nullptr) {
             AP_HAL::panic("Only one megasquirt at a time");