diff --git a/cmake/main.cmake b/cmake/main.cmake
index d8e2dddf3ec..1b987204b25 100644
--- a/cmake/main.cmake
+++ b/cmake/main.cmake
@@ -108,10 +108,10 @@ function(setup_firmware_target exe name)
 endfunction()
 
 function(exclude_from_all target)
-    set_property(TARGET ${target} PROPERTY
+    set_target_properties(${target} PROPERTIES
         TARGET_MESSAGES OFF
-        EXCLUDE_FROM_ALL 1
-        EXCLUDE_FROM_DEFAULT_BUILD 1)
+        EXCLUDE_FROM_ALL ON
+        EXCLUDE_FROM_DEFAULT_BUILD ON)
 endfunction()
 
 function(collect_targets)
diff --git a/cmake/sitl.cmake b/cmake/sitl.cmake
index c94fca21209..9adecab53cc 100644
--- a/cmake/sitl.cmake
+++ b/cmake/sitl.cmake
@@ -19,8 +19,8 @@ main_sources(SITL_SRC
     target/SITL/sim/realFlight.h
     target/SITL/sim/simHelper.c
     target/SITL/sim/simHelper.h
-    target/SITL/sim/simple_soap_client.c
-    target/SITL/sim/simple_soap_client.h
+    target/SITL/sim/soap_client.c
+    target/SITL/sim/soap_client.h
     target/SITL/sim/xplane.c
     target/SITL/sim/xplane.h
 )
@@ -163,8 +163,8 @@ function (target_sitl name)
             WORKING_DIRECTORY ${CMAKE_BINARY_DIR}
             COMMAND ${generator_cmd} clean
             COMMENT "Removing intermediate files for ${name}")
-        set_property(TARGET ${clean_target} PROPERTY
-            EXCLUDE_FROM_ALL 1
-            EXCLUDE_FROM_DEFAULT_BUILD 1)
+        set_target_properties(${clean_target} PROPERTIES
+            EXCLUDE_FROM_ALL ON
+            EXCLUDE_FROM_DEFAULT_BUILD ON)
     endif()
 endfunction()
diff --git a/src/main/config/config_streamer_file.c b/src/main/config/config_streamer_file.c
index 379155c22e1..bce0f5be522 100644
--- a/src/main/config/config_streamer_file.c
+++ b/src/main/config/config_streamer_file.c
@@ -47,7 +47,6 @@ bool configFileSetPath(char* path)
 void config_streamer_impl_unlock(void)
 {
     if (eepromFd != NULL) {
-        fprintf(stderr, "[EEPROM] Unable to load %s\n", eepromPath);
         return;
     }
 
@@ -103,7 +102,6 @@ int config_streamer_impl_write_word(config_streamer_t *c, config_streamer_buffer
 
     if ((c->address >= (uintptr_t)eepromData) && (c->address < (uintptr_t)ARRAYEND(eepromData))) {
         *((uint32_t*)c->address) = *buffer;
-        fprintf(stderr, "[EEPROM] Program word  %p = %08x\n", (void*)c->address, *((uint32_t*)c->address));
     } else {
         fprintf(stderr, "[EEPROM] Program word %p out of range!\n", (void*)c->address);
     }
diff --git a/src/main/scheduler/scheduler.c b/src/main/scheduler/scheduler.c
index e04a92a8993..ad21c32359e 100755
--- a/src/main/scheduler/scheduler.c
+++ b/src/main/scheduler/scheduler.c
@@ -19,6 +19,10 @@
 #include <stdint.h>
 #include <string.h>
 
+#if defined(SITL_BUILD)
+#include <unistd.h>
+#endif
+
 #include "platform.h"
 
 #include "scheduler.h"
@@ -215,11 +219,22 @@ void FAST_CODE NOINLINE scheduler(void)
     uint16_t selectedTaskDynamicPriority = 0;
     bool forcedRealTimeTask = false;
 
+#if defined(SITL_BUILD)
+    // Track the earliest time at which the next task will become due so we can
+    // sleep until then instead of busy-waiting.  Cap at 1 ms so event-driven
+    // tasks (checkFunc) are still polled frequently enough.
+    timeUs_t sitlEarliestNextTaskAt = currentTimeUs + 1000;
+    bool sitlHasCheckFuncTask = false;
+#endif
+
     // Update task dynamic priorities
     uint16_t waitingTasks = 0;
     for (cfTask_t *task = queueFirst(); task != NULL; task = queueNext()) {
         // Task has checkFunc - event driven
         if (task->checkFunc) {
+#if defined(SITL_BUILD)
+            sitlHasCheckFuncTask = true;
+#endif
             const timeUs_t currentTimeBeforeCheckFuncCallUs = micros();
 
             // Increase priority for event driven tasks
@@ -248,7 +263,19 @@ void FAST_CODE NOINLINE scheduler(void)
                 waitingTasks++;
                 forcedRealTimeTask = true;
             }
+#if defined(SITL_BUILD)
+            const timeUs_t taskNextAt = task->lastExecutedAt + (timeUs_t)task->desiredPeriod;
+            if (taskNextAt < sitlEarliestNextTaskAt) {
+                sitlEarliestNextTaskAt = taskNextAt;
+            }            
+#endif
         } else {
+#if defined(SITL_BUILD)
+            const timeUs_t taskNextAt = task->lastExecutedAt + (timeUs_t)task->desiredPeriod;
+            if (taskNextAt < sitlEarliestNextTaskAt) {
+                sitlEarliestNextTaskAt = taskNextAt;
+            }            
+#endif
             // Task is time-driven, dynamicPriority is last execution age (measured in desiredPeriods)
             // Task age is calculated from last execution
             task->taskAgeCycles = ((timeDelta_t)(currentTimeUs - task->lastExecutedAt)) / task->desiredPeriod;
@@ -294,4 +321,27 @@ void FAST_CODE NOINLINE scheduler(void)
         selectedTask->totalExecutionTime += taskExecutionTime;   // time consumed by scheduler + task
         selectedTask->maxExecutionTime = MAX(selectedTask->maxExecutionTime, taskExecutionTime);
     }
+
+#if defined(SITL_BUILD)
+    {
+        // Avoid busy-waiting and burning 100% CPU in SITL.  After executing the
+        // current task (or finding nothing to do), sleep until just before the
+        // next task is due.  For event-driven tasks (checkFunc) we limit the
+        // sleep so the check function is still called frequently.
+        if (sitlHasCheckFuncTask) {
+            // Poll event-driven tasks at least every 500 µs
+            const timeUs_t eventCap = micros() + 500;
+            if (eventCap < sitlEarliestNextTaskAt) {
+                sitlEarliestNextTaskAt = eventCap;
+            }
+        }
+        const timeUs_t nowUs = micros();
+        if (sitlEarliestNextTaskAt > nowUs + 50) {
+            const timeDelta_t sleepUs = (timeDelta_t)(sitlEarliestNextTaskAt - nowUs) - 50;
+            if (sleepUs > 0) {
+                usleep((useconds_t)sleepUs);
+            }
+        }
+    }
+#endif
 }
diff --git a/src/main/target/SITL/sim/realFlight.c b/src/main/target/SITL/sim/realFlight.c
index a88d20a612d..6ff0ff413af 100644
--- a/src/main/target/SITL/sim/realFlight.c
+++ b/src/main/target/SITL/sim/realFlight.c
@@ -28,14 +28,14 @@
 #include <string.h>
 #include <stdarg.h>
 #include <pthread.h>
+#include <stdatomic.h>
 #include <math.h>
 
 #include "platform.h"
 
 #include "target.h"
 #include "target/SITL/sim/realFlight.h"
-#include "target/SITL/sim/simple_soap_client.h"
-#include "target/SITL/sim/xplane.h"
+#include "target/SITL/sim/soap_client.h"
 #include "target/SITL/sim/simHelper.h"
 #include "fc/runtime_config.h"
 #include "drivers/time.h"
@@ -55,8 +55,9 @@
 #include "flight/imu.h"
 #include "io/gps.h"
 #include "rx/sim.h"
+#include "realFlight.h"
 
-#define RF_PORT 18083
+#define RF_PORT "18083"
 #define RF_MAX_CHANNEL_COUNT 12
 
 // "RealFlight Ranch" is located in Sierra Nevada, southern Spain
@@ -67,17 +68,14 @@
 static uint8_t pwmMapping[RF_MAX_PWM_OUTS];
 static uint8_t mappingCount;
 
-static pthread_cond_t sockcond1 = PTHREAD_COND_INITIALIZER;
-static pthread_cond_t sockcond2 = PTHREAD_COND_INITIALIZER;
-static pthread_mutex_t sockmtx = PTHREAD_MUTEX_INITIALIZER;
+static pthread_mutex_t initMutex = PTHREAD_MUTEX_INITIALIZER;
+static pthread_cond_t initCond = PTHREAD_COND_INITIALIZER;
+static atomic_bool shouldStopSoapThread = false;
 
-static soap_client_t *client = NULL;
-static soap_client_t *clientNext = NULL;
+static soap_client_t client;
+static pthread_t soapThread = 0;
 
-static pthread_t soapThread;
-static pthread_t creationThread;
-
-static bool isInitalised = false;
+static atomic_bool isInitalised = false;
 static bool useImu = false;
 
 typedef struct 
@@ -133,38 +131,6 @@ typedef struct
 
 rfValues_t rfValues; 
 
-static void deleteClient(soap_client_t *client)
-{
-    soapClientClose(client);
-    free(client);
-    client = NULL;
-}
-
-static void startRequest(char* action, const char* fmt, ...)
-{
-    pthread_mutex_lock(&sockmtx);
-    while (clientNext == NULL) {
-        pthread_cond_wait(&sockcond1, &sockmtx);
-    }
-
-    client = clientNext;
-    clientNext = NULL;
-
-    pthread_cond_broadcast(&sockcond2);
-    pthread_mutex_unlock(&sockmtx);
-
-    va_list va;
-    va_start(va, fmt);
-    soapClientSendRequestVa(client, action, fmt, va);
-    va_end(va);
-}
-
-static char* endRequest(void)
-{
-   char* ret = soapClientReceive(client); 
-   deleteClient(client);
-   return ret;   
-}
 
 // Simple, but fast ;)
 static double getDoubleFromResponse(const char* response, const char* elementName)
@@ -261,6 +227,7 @@ static float convertAzimuth(float azimuth)
     return 360 - fmodf(azimuth + 90, 360.0f);
 }
 
+
 static void exchangeData(void)
 {
     double servoValues[RF_MAX_PWM_OUTS] = {  };    
@@ -272,14 +239,32 @@ static void exchangeData(void)
         }
     }
 
-    startRequest("ExchangeData", "<ExchangeData><pControlInputs><m-selectedChannels>%u</m-selectedChannels>"
-        "<m-channelValues-0to1><item>%.4f</item><item>%.4f</item><item>%.4f</item><item>%.4f</item><item>%.4f</item><item>%.4f</item><item>%.4f</item><item>%.4f</item>"
-        "<item>%.4f</item><item>%.4f</item><item>%.4f</item><item>%.4f</item></m-channelValues-0to1></pControlInputs></ExchangeData>",
-        0xFFF, 
-        servoValues[0], servoValues[1], servoValues[2], servoValues[3], servoValues[4], servoValues[5], servoValues[6], servoValues[7],
-        servoValues[8], servoValues[9], servoValues[10], servoValues[11]);
-    char* response = endRequest();
+    char requestBody[1024] = "<ExchangeData><pControlInputs><m-selectedChannels>4095</m-selectedChannels><m-channelValues-0to1>";
+    
+    for (int i = 0; i < RF_MAX_CHANNEL_COUNT; i++) {
+        char value[32];
+        snprintf(value, sizeof(value), "<item>%.4f</item>", servoValues[i]);
+        strncat(requestBody, value, sizeof(requestBody) - strlen(requestBody) - 1);
+    }
+    strncat(requestBody, "</m-channelValues-0to1></pControlInputs></ExchangeData>", sizeof(requestBody) - strlen(requestBody) - 1);
+
+    char *response = NULL;
+    int http_status = 0; 
 
+    int ret = soap_client_call_raw_body(
+        &client,
+        "ExchangeData",
+        requestBody,
+        &response,
+        &http_status
+    );
+    
+    if (ret < 0 || http_status != 200 || !response) {
+        fprintf(stderr, "[SIM] Data exchange with RealFlight failed.\n");
+        free(response);
+        return;
+    }
+  
     //rfValues.m_currentPhysicsTime_SEC = getDoubleFromResponse(response, "m-currentPhysicsTime-SEC");
     //rfValues.m_currentPhysicsSpeedMultiplier = getDoubleFromResponse(response, "m-currentPhysicsSpeedMultiplier");
     rfValues.m_airspeed_MPS = getDoubleFromResponse(response, "m-airspeed-MPS");
@@ -410,50 +395,70 @@ static void exchangeData(void)
     free(response);
 }
 
-static void* soapWorker(void* arg)
+static bool restoreOriginalControllerDevice(void)
 {
-    UNUSED(arg);
-    while(true)
-    {     
-        if (!isInitalised) {
-            startRequest("RestoreOriginalControllerDevice", "<RestoreOriginalControllerDevice><a>1</a><b>2</b></RestoreOriginalControllerDevice>");
-            free(endRequest());
-            startRequest("InjectUAVControllerInterface", "<InjectUAVControllerInterface><a>1</a><b>2</b></InjectUAVControllerInterface>");
-            free(endRequest());  
-            exchangeData();
-            ENABLE_ARMING_FLAG(SIMULATOR_MODE_SITL);
-            
-            isInitalised = true;  
-        }
+    char* response = NULL;
+    int http_status = 0;
+    const int ret = soap_client_call_raw_body(
+        &client,
+        "RestoreOriginalControllerDevice",
+        "<RestoreOriginalControllerDevice><a>1</a><b>2</b></RestoreOriginalControllerDevice>",
+        &response,
+        &http_status
+    );
 
-        exchangeData();
-        unlockMainPID();
+    if (ret < 0 || (http_status != 200 && http_status != 500) || !response) {
+        free(response);
+        return false;
     }
 
-    return NULL;
+    free(response);
+    return true;
 }
 
-
-static void* creationWorker(void* arg)
+static void* soapWorker(void* arg)
 {
-    char* ip = (char*)arg;
-    
-    while (true) {
-        pthread_mutex_lock(&sockmtx);
-        while (clientNext != NULL) {
-            pthread_cond_wait(&sockcond2, &sockmtx);
-        }
-        pthread_mutex_unlock(&sockmtx);
-        
-        soap_client_t *cli = malloc(sizeof(soap_client_t));
-        if (!soapClientConnect(cli, ip, RF_PORT)) {
-            continue;
+    UNUSED(arg);
+    while(!atomic_load(&shouldStopSoapThread)) {
+
+        if (!atomic_load(&isInitalised)) {
+            // Initialize RealFlight
+
+            // Alway try to restore the original controller device first to avoid problems with broken connections and the interface being stuck in a half-initialised state. 
+            // RealFlight seems to not properly close the connection on its side if the connection is interrupted, but only after a timeout of about 30 seconds. 
+            // During this time the interface is not usable, but without this step it would be stuck in an unusable state until the next restart of the SITL.
+            if (!restoreOriginalControllerDevice()) {
+                delay(1000);
+                continue;
+            }
+
+            char* response = NULL;
+            int http_status = 0;
+            const int ret = soap_client_call_raw_body(
+                &client,
+                "InjectUAVControllerInterface",
+                "<InjectUAVControllerInterface><a>1</a><b>2</b></InjectUAVControllerInterface>",
+                &response,
+                &http_status
+            );
+
+            if (ret < 0 || http_status != 200 || !response) {
+                free(response);
+                delay(1000);
+                continue; 
+            }
+            
+            exchangeData(); // Get initial data and set initial state in RealFlight
+            
+            ENABLE_ARMING_FLAG(SIMULATOR_MODE_SITL);
+            atomic_store(&isInitalised, true);
+            pthread_mutex_lock(&initMutex);
+            pthread_cond_signal(&initCond);
+            pthread_mutex_unlock(&initMutex);
         }
-        
-        clientNext = cli;
-        pthread_mutex_lock(&sockmtx);
-        pthread_cond_broadcast(&sockcond1);
-        pthread_mutex_unlock(&sockmtx);
+
+        exchangeData();
+        unlockMainPID();
     }
 
     return NULL;
@@ -465,19 +470,46 @@ bool simRealFlightInit(char* ip, uint8_t* mapping, uint8_t mapCount, bool imu)
     mappingCount = mapCount;
     useImu = imu;
 
-    if (pthread_create(&soapThread, NULL, soapWorker, NULL) < 0) {
+    if (soap_client_init(&client, ip, RF_PORT, "/", 1000) < 0) {
         return false;
     }
 
-    if (pthread_create(&creationThread, NULL, creationWorker, (void*)ip) < 0) {
+    atomic_store(&isInitalised, false);
+    atomic_store(&shouldStopSoapThread, false);
+    if (pthread_create(&soapThread, NULL, soapWorker, &client) < 0) {
         return false;
     }
 
     // Wait until the connection is established, the interface has been initialised 
-    // and the first valid packet has been received to avoid problems with the startup calibration.   
-    while (!isInitalised) {
-        delay(250);
+    // and the first valid packet has been received to avoid problems with the startup calibration.
+    pthread_mutex_lock(&initMutex);
+    while (!atomic_load(&isInitalised)) {
+        pthread_cond_wait(&initCond, &initMutex);
     }
+    pthread_mutex_unlock(&initMutex);
 
     return true;
 }
+
+void simRealFlightClose(void)
+{   
+    atomic_store(&shouldStopSoapThread, true);
+    if (soapThread) {
+        pthread_join(soapThread, NULL);
+    }
+
+    if (atomic_load(&isInitalised)) {
+
+        if (!restoreOriginalControllerDevice( )) {
+            fprintf(stderr, "[SIM] Failed to restore original controller device in RealFlight.\n");
+        } else {
+            fprintf(stderr, "[SIM] Restored original controller device in RealFlight.\n");
+        }
+    }
+
+    DISABLE_ARMING_FLAG(SIMULATOR_MODE_SITL);
+    atomic_store(&isInitalised, false);
+    pthread_mutex_destroy(&initMutex);
+    pthread_cond_destroy(&initCond);
+    soap_client_destroy(&client);
+}
diff --git a/src/main/target/SITL/sim/realFlight.h b/src/main/target/SITL/sim/realFlight.h
index af34c2c524e..80e27c9087c 100644
--- a/src/main/target/SITL/sim/realFlight.h
+++ b/src/main/target/SITL/sim/realFlight.h
@@ -27,3 +27,4 @@
 #define RF_MAX_PWM_OUTS 12
 
 bool simRealFlightInit(char* ip, uint8_t* mapping, uint8_t mapCount, bool imu);
+void simRealFlightClose(void);
diff --git a/src/main/target/SITL/sim/simple_soap_client.c b/src/main/target/SITL/sim/simple_soap_client.c
deleted file mode 100644
index e6400ae3b14..00000000000
--- a/src/main/target/SITL/sim/simple_soap_client.c
+++ /dev/null
@@ -1,172 +0,0 @@
-/*
- * This file is part of INAV Project.
- *
- * This Source Code Form is subject to the terms of the Mozilla Public
- * License, v. 2.0. If a copy of the MPL was not distributed with this file,
- * You can obtain one at http://mozilla.org/MPL/2.0/.
- *
- * Alternatively, the contents of this file may be used under the terms
- * of the GNU General Public License Version 3, as described below:
- *
- * This file is free software: you may copy, redistribute and/or modify
- * it under the terms of the GNU General Public License as published by the
- * Free Software Foundation, either version 3 of the License, or (at your
- * option) any later version.
- *
- * This file is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
- * Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see http://www.gnu.org/licenses/.
- */
-
-#define _GNU_SOURCE
-
-#include <stdbool.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <stdarg.h>
-#include <unistd.h>
-#include <string.h>
-#include <sys/socket.h>
-# include <netinet/in.h>
-# include <netdb.h>
-#include <fcntl.h>
-#include <sys/select.h>
-
-#include "simple_soap_client.h"
-
-#define REC_BUF_SIZE 6000
-char recBuffer[REC_BUF_SIZE];
-
-bool soapClientConnect(soap_client_t *client, const char *address, int port)
-{
-    client->sockedFd = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
-    if (client->sockedFd < 0) {
-        return false;
-    }
-
-    int one = 1;
-    if (setsockopt(client->sockedFd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)) < 0) {
-        return false;
-    }
-
-    client->socketAddr.sin_family = AF_INET;
-    client->socketAddr.sin_port = htons(port);
-    client->socketAddr.sin_addr.s_addr = inet_addr(address);
-
-    if (connect(client->sockedFd, (struct sockaddr*)&client->socketAddr, sizeof(client->socketAddr)) < 0) {
-        return false;
-    }
-
-    client->isConnected = true;
-    client->isInitalised = true;
-
-    return true;
-}
-
-void soapClientClose(soap_client_t *client)
-{
-    close(client->sockedFd);
-    memset(client, 0, sizeof(soap_client_t));
-    client->isConnected = false;
-    client->isInitalised = false;
-}
-
-void soapClientSendRequestVa(soap_client_t *client, const char* action, const char *fmt, va_list va)
-{
-     if (!client->isConnected) {
-        return;
-    }
-
-    char* requestBody;
-    if (vasprintf(&requestBody, fmt, va) < 0) {
-        return;
-    }
-
-    char* request;
-    if (asprintf(&request, "POST / HTTP/1.1\r\nsoapaction: %s\r\ncontent-length: %u\r\ncontent-type: text/xml;charset='UTF-8'\r\nConnection: Keep-Alive\r\n\r\n<soap:Envelope xmlns:xsi=\"http://www.w3.org/2001/XMLSchema-instance\" xmlns:xsd=\"http://www.w3.org/2001/XMLSchema\" xmlns:soap=\"http://schemas.xmlsoap.org/soap/envelope/\"><soap:Body>%s</soap:Body></soap:Envelope>",
-         action, (unsigned)strlen(requestBody), requestBody) < 0) {
-            return;
-         }
-
-    send(client->sockedFd, request, strlen(request), 0);
-
-    free(requestBody);
-    free(request);
-}
-
-void soapClientSendRequest(soap_client_t *client, const char* action, const char *fmt, ...)
-{
-    va_list va;
-
-    va_start(va, fmt);
-    soapClientSendRequestVa(client, action, fmt, va);
-    va_end(va);
-}
-
-static bool soapClientPoll(soap_client_t *client, uint32_t timeout_ms)
-{
-    fd_set fds;
-    struct timeval tv;
-
-    FD_ZERO(&fds);
-    FD_SET(client->sockedFd, &fds);
-
-    tv.tv_sec = timeout_ms / 1000;
-    tv.tv_usec = (timeout_ms % 1000) * 1000UL;
-
-    if (select(client->sockedFd + 1, &fds, NULL, NULL, &tv) != 1) {
-        return false;
-    }
-    return true;
-}
-
-
-char* soapClientReceive(soap_client_t *client)
-{
-     if (!client->isInitalised){
-        return false;
-    }
-
-    if (!soapClientPoll(client, 1000)) {
-        return false;
-    }
-
-    ssize_t size = recv(client->sockedFd, recBuffer, REC_BUF_SIZE, 0);
-
-    if (size <= 0) {
-        return NULL;
-    }
-
-    char* pos = strstr(recBuffer, "Content-Length: ");
-    if (!pos) {
-       return NULL;
-    }
-
-    uint32_t contentLength = strtoul(pos + 16, NULL, 10);
-    char *body = strstr(pos, "\r\n\r\n");
-    if (!body) {
-        return NULL;
-    }
-
-    body += 4;
-
-    ssize_t expectedLength = contentLength + body - recBuffer;
-    if ((unsigned)expectedLength >= sizeof(recBuffer)) {
-        return NULL;
-    }
-
-    while (size < expectedLength){
-        ssize_t size2 = recv(client->sockedFd, &recBuffer[size], sizeof(recBuffer - size + 1), 0);
-        if (size2 <= 0) {
-            return NULL;
-        }
-        size += size2;
-    }
-
-    recBuffer[size] = '\0';
-    return strdup(body);
-}
diff --git a/src/main/target/SITL/sim/soap_client.c b/src/main/target/SITL/sim/soap_client.c
new file mode 100644
index 00000000000..6ac5f65826a
--- /dev/null
+++ b/src/main/target/SITL/sim/soap_client.c
@@ -0,0 +1,516 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#define _POSIX_C_SOURCE 200809L
+
+#include "soap_client.h"
+
+#include <arpa/inet.h>
+#include <errno.h>
+#include <fcntl.h>
+#include <netdb.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/socket.h>
+#include <sys/time.h>
+#include <unistd.h>
+
+static int set_socket_timeout(int fd, int timeout_ms) {
+    struct timeval tv;
+    tv.tv_sec = timeout_ms / 1000;
+    tv.tv_usec = (timeout_ms % 1000) * 1000;
+
+    if (setsockopt(fd, SOL_SOCKET, SO_SNDTIMEO, &tv, sizeof(tv)) != 0) {
+        return -1;
+    }
+    if (setsockopt(fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)) != 0) {
+        return -1;
+    }
+    return 0;
+}
+
+static int connect_with_timeout(const struct addrinfo* ai, int timeout_ms) {
+    int fd = socket(ai->ai_family, ai->ai_socktype, ai->ai_protocol);
+    if (fd < 0) {
+        return -1;
+    }
+
+    int flags = fcntl(fd, F_GETFL, 0);
+    if (flags < 0) {
+        close(fd);
+        return -1;
+    }
+    if (fcntl(fd, F_SETFL, flags | O_NONBLOCK) < 0) {
+        close(fd);
+        return -1;
+    }
+
+    int one = 1;
+    if (setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)) != 0) {
+        close(fd);
+        return -1;
+    }
+
+    int rc = connect(fd, ai->ai_addr, ai->ai_addrlen);
+    if (rc == 0) {
+        (void)fcntl(fd, F_SETFL, flags);
+        return fd;
+    }
+    if (errno != EINPROGRESS) {
+        close(fd);
+        return -1;
+    }
+
+    fd_set wfds;
+    FD_ZERO(&wfds);
+    FD_SET(fd, &wfds);
+    struct timeval tv;
+    tv.tv_sec = timeout_ms / 1000;
+    tv.tv_usec = (timeout_ms % 1000) * 1000;
+
+    rc = select(fd + 1, NULL, &wfds, NULL, &tv);
+    if (rc <= 0) {
+        close(fd);
+        errno = (rc == 0) ? ETIMEDOUT : errno;
+        return -1;
+    }
+
+    int so_error = 0;
+    socklen_t so_error_len = sizeof(so_error);
+    if (getsockopt(fd, SOL_SOCKET, SO_ERROR, &so_error, &so_error_len) != 0 || so_error != 0) {
+        close(fd);
+        errno = (so_error != 0) ? so_error : errno;
+        return -1;
+    }
+
+    if (fcntl(fd, F_SETFL, flags) < 0) {
+        close(fd);
+        return -1;
+    }
+
+    return fd;
+}
+
+static int open_tcp_connection(const char* host, const char* port, int timeout_ms) {
+    struct addrinfo hints;
+    memset(&hints, 0, sizeof(hints));
+    hints.ai_family = AF_UNSPEC;
+    hints.ai_socktype = SOCK_STREAM;
+
+    struct addrinfo* result = NULL;
+    if (getaddrinfo(host, port, &hints, &result) != 0) {
+        return -1;
+    }
+
+    int fd = -1;
+    for (const struct addrinfo* ai = result; ai != NULL; ai = ai->ai_next) {
+        fd = connect_with_timeout(ai, timeout_ms);
+        if (fd >= 0) {
+            if (set_socket_timeout(fd, timeout_ms) == 0) {
+                break;
+            }
+            close(fd);
+            fd = -1;
+        }
+    }
+
+    freeaddrinfo(result);
+    return fd;
+}
+
+static int send_all(int fd, const char* data, size_t len) {
+    size_t total = 0;
+    while (total < len) {
+        ssize_t n = send(fd, data + total, len - total, 0);
+        if (n < 0) {
+            if (errno == EINTR) {
+                continue;
+            }
+            return -1;
+        }
+        if (n == 0) {
+            return -1;
+        }
+        total += (size_t)n;
+    }
+    return 0;
+}
+
+static char* recv_all(int fd, size_t* out_len) {
+    size_t cap = 8192;
+    size_t len = 0;
+    char* buf = (char*)malloc(cap);
+    if (!buf) {
+        return NULL;
+    }
+
+    while (1) {
+        if (len == cap) {
+            size_t new_cap = cap * 2;
+            char* next = (char*)realloc(buf, new_cap);
+            if (!next) {
+                free(buf);
+                return NULL;
+            }
+            buf = next;
+            cap = new_cap;
+        }
+
+        ssize_t n = recv(fd, buf + len, cap - len, 0);
+        if (n < 0) {
+            if (errno == EINTR) {
+                continue;
+            }
+            free(buf);
+            return NULL;
+        }
+        if (n == 0) {
+            break;
+        }
+        len += (size_t)n;
+    }
+
+    char* out = (char*)realloc(buf, len + 1);
+    if (!out) {
+        free(buf);
+        return NULL;
+    }
+    out[len] = '\0';
+    *out_len = len;
+    return out;
+}
+
+static int parse_http_status(const char* response) {
+    int status = 0;
+    if (sscanf(response, "HTTP/%*d.%*d %d", &status) == 1) {
+        return status;
+    }
+    return 0;
+}
+
+static char* dechunk_http_body(const char* body, size_t body_len, size_t* out_len) {
+    size_t cap = body_len + 1;
+    char* out = (char*)malloc(cap);
+    if (!out) {
+        return NULL;
+    }
+    size_t out_pos = 0;
+    size_t pos = 0;
+
+    while (pos < body_len) {
+        const char* line_end = strstr(body + pos, "\r\n");
+        if (!line_end) {
+            free(out);
+            return NULL;
+        }
+
+        char size_buf[32];
+        size_t size_len = (size_t)(line_end - (body + pos));
+        if (size_len == 0 || size_len >= sizeof(size_buf)) {
+            free(out);
+            return NULL;
+        }
+        memcpy(size_buf, body + pos, size_len);
+        size_buf[size_len] = '\0';
+
+        char* endptr = NULL;
+        unsigned long chunk_size = strtoul(size_buf, &endptr, 16);
+        if (endptr == size_buf) {
+            free(out);
+            return NULL;
+        }
+
+        pos = (size_t)(line_end - body) + 2;
+        if (chunk_size == 0) {
+            break;
+        }
+        if (pos + chunk_size + 2 > body_len) {
+            free(out);
+            return NULL;
+        }
+
+        if (out_pos + chunk_size + 1 > cap) {
+            size_t new_cap = cap;
+            while (out_pos + chunk_size + 1 > new_cap) {
+                new_cap *= 2;
+            }
+            char* next = (char*)realloc(out, new_cap);
+            if (!next) {
+                free(out);
+                return NULL;
+            }
+            out = next;
+            cap = new_cap;
+        }
+
+        memcpy(out + out_pos, body + pos, chunk_size);
+        out_pos += chunk_size;
+        pos += chunk_size;
+
+        if (!(body[pos] == '\r' && body[pos + 1] == '\n')) {
+            free(out);
+            return NULL;
+        }
+        pos += 2;
+    }
+
+    out[out_pos] = '\0';
+    *out_len = out_pos;
+    return out;
+}
+
+static char* extract_soap_body_raw(const char* xml, size_t xml_len) {
+    const char* p = xml;
+    const char* end = xml + xml_len;
+
+    const char* body_open = NULL;
+    const char* body_name_start = NULL;
+    const char* body_name_end = NULL;
+
+    while (p < end) {
+        const char* lt = strchr(p, '<');
+        if (!lt || lt >= end) {
+            break;
+        }
+        if (lt + 1 < end && (lt[1] == '/' || lt[1] == '?' || lt[1] == '!')) {
+            p = lt + 1;
+            continue;
+        }
+        const char* gt = strchr(lt, '>');
+        if (!gt || gt >= end) {
+            break;
+        }
+
+        const char* name_start = lt + 1;
+        const char* name_end = name_start;
+        while (name_end < gt && *name_end != ' ' && *name_end != '\t' && *name_end != '\r' && *name_end != '\n' && *name_end != '/') {
+            name_end++;
+        }
+
+        size_t name_len = (size_t)(name_end - name_start);
+        if (name_len >= 4 && strncmp(name_end - 4, "Body", 4) == 0) {
+            body_open = gt + 1;
+            body_name_start = name_start;
+            body_name_end = name_end;
+            break;
+        }
+        p = gt + 1;
+    }
+
+    if (!body_open || !body_name_start || !body_name_end) {
+        return NULL;
+    }
+
+    char close_tag[128];
+    size_t tag_len = (size_t)(body_name_end - body_name_start);
+    if (tag_len + 4 >= sizeof(close_tag)) {
+        return NULL;
+    }
+    close_tag[0] = '<';
+    close_tag[1] = '/';
+    memcpy(close_tag + 2, body_name_start, tag_len);
+    close_tag[tag_len + 2] = '>';
+    close_tag[tag_len + 3] = '\0';
+
+    const char* close = strstr(body_open, close_tag);
+    if (!close) {
+        return NULL;
+    }
+
+    size_t inner_len = (size_t)(close - body_open);
+    char* inner = (char*)malloc(inner_len + 1);
+    if (!inner) {
+        return NULL;
+    }
+    memcpy(inner, body_open, inner_len);
+    inner[inner_len] = '\0';
+    return inner;
+}
+
+int soap_client_init(soap_client_t* c, const char* host, const char* port, const char* path, int timeout_ms) {
+    if (!c || !host || !port || !path) {
+        return -1;
+    }
+    memset(c, 0, sizeof(*c));
+    if (snprintf(c->host, sizeof(c->host), "%s", host) >= (int)sizeof(c->host)) {
+        return -1;
+    }
+    if (snprintf(c->port, sizeof(c->port), "%s", port) >= (int)sizeof(c->port)) {
+        return -1;
+    }
+    if (snprintf(c->path, sizeof(c->path), "%s", path) >= (int)sizeof(c->path)) {
+        return -1;
+    }
+    c->timeout_ms = timeout_ms;
+    return pthread_mutex_init(&c->lock, NULL);
+}
+
+void soap_client_destroy(soap_client_t* c) {
+    if (c) {
+        pthread_mutex_destroy(&c->lock);
+    }
+}
+
+int soap_client_call_raw_body(soap_client_t* c,
+                              const char* soap_action,
+                              const char* request_body_xml,
+                              char** response_body_xml,
+                              int* http_status) {
+    if (!c || !request_body_xml || !response_body_xml || !http_status) {
+        return -1;
+    }
+
+    char host[256];
+    char port[16];
+    char path[256];
+    int timeout_ms = 0;
+
+    pthread_mutex_lock(&c->lock);
+    snprintf(host, sizeof(host), "%s", c->host);
+    snprintf(port, sizeof(port), "%s", c->port);
+    snprintf(path, sizeof(path), "%s", c->path);
+    timeout_ms = c->timeout_ms;
+    pthread_mutex_unlock(&c->lock);
+
+    const char* envelope_prefix =
+        "<?xml version=\"1.0\" encoding=\"utf-8\"?>"
+        "<soap:Envelope xmlns:soap=\"http://schemas.xmlsoap.org/soap/envelope/\">"
+        "<soap:Body>";
+    const char* envelope_suffix = "</soap:Body></soap:Envelope>";
+
+    size_t body_len = strlen(request_body_xml);
+    size_t envelope_len = strlen(envelope_prefix) + body_len + strlen(envelope_suffix);
+    char* envelope = (char*)malloc(envelope_len + 1);
+    if (!envelope) {
+        return -1;
+    }
+    snprintf(envelope, envelope_len + 1, "%s%s%s", envelope_prefix, request_body_xml, envelope_suffix);
+
+    const char* action_header = (soap_action && soap_action[0] != '\0') ? soap_action : "";
+    int req_len = snprintf(NULL,
+                           0,
+                           "POST %s HTTP/1.1\r\n"
+                           "Host: %s:%s\r\n"
+                           "Content-Type: text/xml; charset=utf-8\r\n"
+                           "SOAPAction: \"%s\"\r\n"
+                           "Content-Length: %zu\r\n"
+                           "Connection: close\r\n\r\n%s",
+                           path,
+                           host,
+                           port,
+                           action_header,
+                           envelope_len,
+                           envelope);
+    if (req_len < 0) {
+        free(envelope);
+        return -1;
+    }
+
+    char* request = (char*)malloc((size_t)req_len + 1);
+    if (!request) {
+        free(envelope);
+        return -1;
+    }
+    snprintf(request,
+             (size_t)req_len + 1,
+             "POST %s HTTP/1.1\r\n"
+             "Host: %s:%s\r\n"
+             "Content-Type: text/xml; charset=utf-8\r\n"
+             "SOAPAction: \"%s\"\r\n"
+             "Content-Length: %zu\r\n"
+             "Connection: close\r\n\r\n%s",
+             path,
+             host,
+             port,
+             action_header,
+             envelope_len,
+             envelope);
+
+    int fd = open_tcp_connection(host, port, timeout_ms);
+    if (fd < 0) {
+        free(request);
+        free(envelope);
+        return -1;
+    }
+
+    int rc = 0;
+    size_t resp_len = 0;
+    char* response = NULL;
+    char* body = NULL;
+    char* soap_body = NULL;
+
+    if (send_all(fd, request, (size_t)req_len) != 0) {
+        rc = -1;
+        goto cleanup;
+    }
+
+    response = recv_all(fd, &resp_len);
+    if (!response || resp_len == 0) {
+        rc = -1;
+        goto cleanup;
+    }
+
+    *http_status = parse_http_status(response);
+
+    char* headers_end = strstr(response, "\r\n\r\n");
+    if (!headers_end) {
+        rc = -1;
+        goto cleanup;
+    }
+    char* raw_body = headers_end + 4;
+    size_t raw_body_len = resp_len - (size_t)(raw_body - response);
+
+    if (strstr(response, "Transfer-Encoding: chunked") || strstr(response, "transfer-encoding: chunked")) {
+        size_t decoded_len = 0;
+        body = dechunk_http_body(raw_body, raw_body_len, &decoded_len);
+        if (!body) {
+            rc = -1;
+            goto cleanup;
+        }
+        soap_body = extract_soap_body_raw(body, decoded_len);
+    } else {
+        soap_body = extract_soap_body_raw(raw_body, raw_body_len);
+    }
+
+    if (!soap_body) {
+        rc = -1;
+        goto cleanup;
+    }
+
+    *response_body_xml = soap_body;
+    soap_body = NULL;
+
+cleanup:
+    if (fd >= 0) {
+        close(fd);
+    }
+    free(request);
+    free(envelope);
+    free(response);
+    free(body);
+    free(soap_body);
+    return rc;
+}
diff --git a/src/main/target/SITL/sim/simple_soap_client.h b/src/main/target/SITL/sim/soap_client.h
similarity index 60%
rename from src/main/target/SITL/sim/simple_soap_client.h
rename to src/main/target/SITL/sim/soap_client.h
index 8522b28859a..ae8cd8cedcf 100644
--- a/src/main/target/SITL/sim/simple_soap_client.h
+++ b/src/main/target/SITL/sim/soap_client.h
@@ -22,27 +22,25 @@
  * along with this program. If not, see http://www.gnu.org/licenses/.
  */
 
-#include <arpa/inet.h>
-#include <netinet/in.h>
-#include <netdb.h>
+#pragma once
 
-#define SOAP_REC_BUF_SIZE 256 * 1024
+#include <pthread.h>
 
 typedef struct {
-    int sockedFd;
-    struct sockaddr_in socketAddr;
-    bool isInitalised;
-    bool isConnected;
+    char host[256];
+    char port[16];
+    char path[256];
+    int timeout_ms;
+    pthread_mutex_t lock;
 } soap_client_t;
 
-typedef struct {
-    soap_client_t client;
-    char* content;
-} send_info_t;
-
+int soap_client_init(soap_client_t* c, const char* host, const char* port, const char* path, int timeout_ms);
+void soap_client_destroy(soap_client_t* c);
 
-bool soapClientConnect(soap_client_t *client, const char *address, int port);
-void soapClientClose(soap_client_t *client);
-void soapClientSendRequestVa(soap_client_t *client, const char* action, const char *fmt, va_list va);
-void soapClientSendRequest(soap_client_t *client, const char* action, const char *fmt, ...);
-char* soapClientReceive(soap_client_t *client);
+// Sends raw XML as content of <soap:Body> and returns raw XML content of response <soap:Body>.
+// Caller must free(*response_body_xml) on success.
+int soap_client_call_raw_body(soap_client_t* c,
+                              const char* soap_action,
+                              const char* request_body_xml,
+                              char** response_body_xml,
+                              int* http_status);
\ No newline at end of file
diff --git a/src/main/target/SITL/sim/xplane.c b/src/main/target/SITL/sim/xplane.c
index 612f4a54a0a..bf0349c47d1 100644
--- a/src/main/target/SITL/sim/xplane.c
+++ b/src/main/target/SITL/sim/xplane.c
@@ -33,8 +33,10 @@
 #include <sys/types.h>
 #include <netinet/in.h>
 #include <pthread.h>
+#include <stdatomic.h>
 #include <errno.h>
 #include <math.h>
+#include <unistd.h>
 
 #include "platform.h"
 
@@ -58,6 +60,7 @@
 #include "flight/imu.h"
 #include "io/gps.h"
 #include "rx/sim.h"
+#include "xplane.h"
 
 #define XP_PORT 49000
 #define XPLANE_JOYSTICK_AXIS_COUNT 8
@@ -66,11 +69,15 @@
 static uint8_t pwmMapping[XP_MAX_PWM_OUTS];
 static uint8_t mappingCount;
 
+static pthread_mutex_t initMutex = PTHREAD_MUTEX_INITIALIZER;
+static pthread_cond_t initCond = PTHREAD_COND_INITIALIZER;
+static atomic_bool shouldStopListenThread = false;
+
 static struct sockaddr_storage serverAddr;
 static socklen_t serverAddrLen;
 static int sockFd;
-static pthread_t listenThread;
-static bool initialized = false;
+static pthread_t listenThread = 0;
+static atomic_bool initalized = false;
 static bool useImu = false;
 
 static float lattitude = 0;
@@ -170,6 +177,40 @@ static void sendDref(char* dref, float value)
     sendto(sockFd, (void*)buf, sizeof(buf), 0, (struct sockaddr*)&serverAddr, serverAddrLen);
 }
 
+static void tryRegisterDrefs(void)
+{
+    registerDref(DREF_LATITUDE, "sim/flightmodel/position/latitude", 100);
+    registerDref(DREF_LONGITUDE, "sim/flightmodel/position/longitude", 100);
+    registerDref(DREF_ELEVATION, "sim/flightmodel/position/elevation", 100);
+    registerDref(DREF_AGL, "sim/flightmodel/position/y_agl", 100);
+    registerDref(DREF_LOCAL_VX, "sim/flightmodel/position/local_vx", 100);
+    registerDref(DREF_LOCAL_VY, "sim/flightmodel/position/local_vy", 100);
+    registerDref(DREF_LOCAL_VZ, "sim/flightmodel/position/local_vz", 100);
+    registerDref(DREF_GROUNDSPEED, "sim/flightmodel/position/groundspeed", 100);
+    registerDref(DREF_TRUE_AIRSPEED, "sim/flightmodel/position/true_airspeed", 100);
+    registerDref(DREF_POS_PHI, "sim/flightmodel/position/phi", 100);
+    registerDref(DREF_POS_THETA, "sim/flightmodel/position/theta", 100);
+    registerDref(DREF_POS_PSI, "sim/flightmodel/position/psi", 100);
+    registerDref(DREF_POS_HPATH, "sim/flightmodel/position/hpath", 100);
+    registerDref(DREF_FORCE_G_AXI1, "sim/flightmodel/forces/g_axil", 100);
+    registerDref(DREF_FORCE_G_SIDE, "sim/flightmodel/forces/g_side", 100);
+    registerDref(DREF_FORCE_G_NRML, "sim/flightmodel/forces/g_nrml", 100);
+    registerDref(DREF_POS_P, "sim/flightmodel/position/P", 100);
+    registerDref(DREF_POS_Q, "sim/flightmodel/position/Q", 100);
+    registerDref(DREF_POS_R, "sim/flightmodel/position/R", 100);
+    registerDref(DREF_POS_BARO_CURRENT_INHG, "sim/weather/barometer_current_inhg", 100);
+    registerDref(DREF_HAS_JOYSTICK, "sim/joystick/has_joystick", 100);
+    registerDref(DREF_JOYSTICK_VALUES_PITCH, "sim/joystick/joy_mapped_axis_value[1]", 100);
+    registerDref(DREF_JOYSTICK_VALUES_ROll, "sim/joystick/joy_mapped_axis_value[2]", 100);
+    registerDref(DREF_JOYSTICK_VALUES_YAW, "sim/joystick/joy_mapped_axis_value[3]", 100);
+    // Abusing cowl flaps for other channels
+    registerDref(DREF_JOYSTICK_VALUES_THROTTLE, "sim/joystick/joy_mapped_axis_value[57]", 100);
+    registerDref(DREF_JOYSTICK_VALUES_CH5, "sim/joystick/joy_mapped_axis_value[58]", 100);
+    registerDref(DREF_JOYSTICK_VALUES_CH6, "sim/joystick/joy_mapped_axis_value[59]", 100);
+    registerDref(DREF_JOYSTICK_VALUES_CH7, "sim/joystick/joy_mapped_axis_value[60]", 100);
+    registerDref(DREF_JOYSTICK_VALUES_CH8, "sim/joystick/joy_mapped_axis_value[61]", 100);
+}
+
 static void* listenWorker(void* arg)
 {
     UNUSED(arg);
@@ -179,8 +220,11 @@ static void* listenWorker(void* arg)
     socklen_t slen = sizeof(remoteAddr);
     int recvLen;
 
-    while (true)
+    while (!atomic_load(&shouldStopListenThread))
     {
+        if (!atomic_load(&initalized)) {
+            tryRegisterDrefs();
+        }
 
         float motorValue = 0;
         float yokeValues[3] = { 0 };
@@ -210,10 +254,12 @@ static void* listenWorker(void* arg)
 
         recvLen = recvfrom(sockFd, buf, sizeof(buf), 0, (struct sockaddr*)&remoteAddr, &slen);
         if (recvLen < 0 && errno != EWOULDBLOCK) {
+            delay(250);
             continue;
         }
 
         if (strncmp((char*)buf, "RREF", 4) != 0) {
+            delay(250);
             continue;
         }
 
@@ -426,11 +472,14 @@ static void* listenWorker(void* arg)
             constrainToInt16(north.z * 1024.0f)
         );
 
-        if (!initialized) {
+        if (!atomic_load(&initalized)) {
             ENABLE_ARMING_FLAG(SIMULATOR_MODE_SITL);
             // Aircraft can wobble on the runway and prevents calibration of the accelerometer
             ENABLE_STATE(ACCELEROMETER_CALIBRATED);
-            initialized = true;
+            atomic_store(&initalized, true);
+            pthread_mutex_lock(&initMutex);
+            pthread_cond_signal(&initCond);
+            pthread_mutex_unlock(&initMutex);
         }
 
         unlockMainPID();
@@ -458,7 +507,7 @@ bool simXPlaneInit(char* ip, int port, uint8_t* mapping, uint8_t mapCount, bool
     if (sockFd < 0) {
         return false;
     } else {
-	char addrbuf[IPADDRESS_PRINT_BUFLEN];
+        char addrbuf[IPADDRESS_PRINT_BUFLEN];
         char *nptr = prettyPrintAddress((struct sockaddr *)&serverAddr, addrbuf, IPADDRESS_PRINT_BUFLEN );
         if (nptr != NULL) {
             fprintf(stderr, "[SOCKET] xplane address = %s, fd=%d\n", nptr, sockFd);
@@ -466,9 +515,9 @@ bool simXPlaneInit(char* ip, int port, uint8_t* mapping, uint8_t mapCount, bool
     }
 
     struct timeval tv;
-        tv.tv_sec = 1;
-        tv.tv_usec = 0;
-        if (setsockopt(sockFd, SOL_SOCKET, SO_RCVTIMEO, (struct timeval *) &tv,sizeof(struct timeval))) {
+    tv.tv_sec = 1;
+    tv.tv_usec = 0;
+    if (setsockopt(sockFd, SOL_SOCKET, SO_RCVTIMEO, (struct timeval *) &tv,sizeof(struct timeval))) {
         return false;
     }
 
@@ -476,43 +525,30 @@ bool simXPlaneInit(char* ip, int port, uint8_t* mapping, uint8_t mapCount, bool
         return false;
     }
 
+    atomic_store(&initalized, false);
+    atomic_store(&shouldStopListenThread, false);
     if (pthread_create(&listenThread, NULL, listenWorker, NULL) < 0) {
         return false;
     }
 
-    while (!initialized) {
-        registerDref(DREF_LATITUDE, "sim/flightmodel/position/latitude", 100);
-        registerDref(DREF_LONGITUDE, "sim/flightmodel/position/longitude", 100);
-        registerDref(DREF_ELEVATION, "sim/flightmodel/position/elevation", 100);
-        registerDref(DREF_AGL, "sim/flightmodel/position/y_agl", 100);
-        registerDref(DREF_LOCAL_VX, "sim/flightmodel/position/local_vx", 100);
-        registerDref(DREF_LOCAL_VY, "sim/flightmodel/position/local_vy", 100);
-        registerDref(DREF_LOCAL_VZ, "sim/flightmodel/position/local_vz", 100);
-        registerDref(DREF_GROUNDSPEED, "sim/flightmodel/position/groundspeed", 100);
-        registerDref(DREF_TRUE_AIRSPEED, "sim/flightmodel/position/true_airspeed", 100);
-        registerDref(DREF_POS_PHI, "sim/flightmodel/position/phi", 100);
-        registerDref(DREF_POS_THETA, "sim/flightmodel/position/theta", 100);
-        registerDref(DREF_POS_PSI, "sim/flightmodel/position/psi", 100);
-        registerDref(DREF_POS_HPATH, "sim/flightmodel/position/hpath", 100);
-        registerDref(DREF_FORCE_G_AXI1, "sim/flightmodel/forces/g_axil", 100);
-        registerDref(DREF_FORCE_G_SIDE, "sim/flightmodel/forces/g_side", 100);
-        registerDref(DREF_FORCE_G_NRML, "sim/flightmodel/forces/g_nrml", 100);
-        registerDref(DREF_POS_P, "sim/flightmodel/position/P", 100);
-        registerDref(DREF_POS_Q, "sim/flightmodel/position/Q", 100);
-        registerDref(DREF_POS_R, "sim/flightmodel/position/R", 100);
-        registerDref(DREF_POS_BARO_CURRENT_INHG, "sim/weather/barometer_current_inhg", 100);
-        registerDref(DREF_HAS_JOYSTICK, "sim/joystick/has_joystick", 100);
-        registerDref(DREF_JOYSTICK_VALUES_PITCH, "sim/joystick/joy_mapped_axis_value[1]", 100);
-        registerDref(DREF_JOYSTICK_VALUES_ROll, "sim/joystick/joy_mapped_axis_value[2]", 100);
-        registerDref(DREF_JOYSTICK_VALUES_YAW, "sim/joystick/joy_mapped_axis_value[3]", 100);
-        // Abusing cowl flaps for other channels
-        registerDref(DREF_JOYSTICK_VALUES_THROTTLE, "sim/joystick/joy_mapped_axis_value[57]", 100);
-        registerDref(DREF_JOYSTICK_VALUES_CH5, "sim/joystick/joy_mapped_axis_value[58]", 100);
-        registerDref(DREF_JOYSTICK_VALUES_CH6, "sim/joystick/joy_mapped_axis_value[59]", 100);
-        registerDref(DREF_JOYSTICK_VALUES_CH7, "sim/joystick/joy_mapped_axis_value[60]", 100);
-        registerDref(DREF_JOYSTICK_VALUES_CH8, "sim/joystick/joy_mapped_axis_value[61]", 100);
-        delay(250);
+    pthread_mutex_lock(&initMutex);
+    while (!atomic_load(&initalized)) {
+        pthread_cond_wait(&initCond, &initMutex);        
     }
+    pthread_mutex_unlock(&initMutex);
 
     return true;
 }
+
+void simXPlaneClose(void)
+{
+        atomic_store(&shouldStopListenThread, true);
+        if (listenThread) {
+            pthread_join(listenThread, NULL);
+        }
+        DISABLE_ARMING_FLAG(SIMULATOR_MODE_SITL);
+        atomic_store(&initalized, false);
+        pthread_mutex_destroy(&initMutex);
+        pthread_cond_destroy(&initCond);
+        close(sockFd);
+}
diff --git a/src/main/target/SITL/sim/xplane.h b/src/main/target/SITL/sim/xplane.h
index 1777a30af2b..619bef615e0 100644
--- a/src/main/target/SITL/sim/xplane.h
+++ b/src/main/target/SITL/sim/xplane.h
@@ -27,3 +27,4 @@
 #define XP_MAX_PWM_OUTS 4
 
 bool simXPlaneInit(char* ip, int port, uint8_t* mapping, uint8_t mapCount, bool imu);
+void simXPlaneClose(void);
diff --git a/src/main/target/SITL/target.c b/src/main/target/SITL/target.c
index bb34f2cd665..2542a352bc1 100644
--- a/src/main/target/SITL/target.c
+++ b/src/main/target/SITL/target.c
@@ -39,6 +39,7 @@
 #include <arpa/inet.h>
 #include <netdb.h>
 #include <netinet/in.h>
+#include <signal.h>
 
 #include <platform.h>
 #include "target.h"
@@ -81,6 +82,25 @@ static void printVersion(void) {
     fprintf(stderr, "INAV %d.%d.%d SITL (%s)\n", FC_VERSION_MAJOR, FC_VERSION_MINOR, FC_VERSION_PATCH_LEVEL, shortGitRevision);
 }
 
+static void cleanupAndExit(int code, bool shouldExit) {
+    if (sitlSim == SITL_SIM_XPLANE) {
+        simXPlaneClose();
+    } else if (sitlSim == SITL_SIM_REALFLIGHT) {
+        simRealFlightClose();
+    }
+    pthread_mutex_destroy(&mainLoopLock);
+
+    if (shouldExit) {
+        exit(code);
+    }
+}
+
+static void on_sigint(int sig) {
+    UNUSED(sig);
+    fprintf(stderr, "\n[SYSTEM] Caught SIGINT, exiting...\n");
+    cleanupAndExit(0, true);
+}
+
 void systemInit(void) {
     printVersion();
     clock_gettime(CLOCK_MONOTONIC, &start_time);
@@ -101,6 +121,12 @@ void systemInit(void) {
         exit(1);
     }
 
+    struct sigaction sa;
+    sa.sa_handler = on_sigint;
+    sigemptyset(&sa.sa_mask);
+    sa.sa_flags = 0;
+    sigaction(SIGINT, &sa, NULL);
+
     if (sitlSim != SITL_SIM_NONE) {
         fprintf(stderr, "[SIM] Waiting for connection...\n");
     }
@@ -390,6 +416,7 @@ void delay(timeMs_t ms)
 void systemReset(void)
 {
     fprintf(stderr, "[SYSTEM] Reset\n");
+    cleanupAndExit(0, false);
 #if defined(__CYGWIN__) || defined(__APPLE__) || GCC_MAJOR < 12
     for(int j = 3; j < 1024; j++) {
         close(j);
@@ -404,7 +431,7 @@ void systemReset(void)
 void systemResetToBootloader(void)
 {
     fprintf(stderr, "[SYSTEM] Reset to bootloader\n");
-    exit(0);
+    cleanupAndExit(0, true);
 }
 
 void failureMode(failureMode_e mode) {