- Bugfixes

- Add magnetometer and rnagefinder
- Add joystick interface
- Configuator integration
- Update docs

.github/workflows/ci.yml

@@ -43,6 +43,36 @@ jobs:
           name: ${{ env.BUILD_NAME }}.zip
           path: ./build/*.hex
 
+  build-SITL:
+    runs-on: ubuntu-latest
+    steps:
+      - uses: actions/checkout@v3
+      - name: Install dependencies
+        run: sudo apt-get update && sudo apt-get -y install ninja-build
+      - name: Setup environment
+        env:
+          ACTIONS_ALLOW_UNSECURE_COMMANDS: true
+        run: |
+          # This is the hash of the commit for the PR
+          # when the action is triggered by PR, empty otherwise
+          COMMIT_ID=${{ github.event.pull_request.head.sha }}
+          # This is the hash of the commit when triggered by push
+          # but the hash of refs/pull/<n>/merge, which is different
+          # from the hash of the latest commit in the PR, that's
+          # why we try github.event.pull_request.head.sha first
+          COMMIT_ID=${COMMIT_ID:-${{ github.sha }}}
+          BUILD_SUFFIX=ci-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID})
+          VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/^[ \t]+|[ \t\)]+$/, "", $2); print $2 }')
+          echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV
+          echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV
+      - name: Build SITL
+        run: mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja .. && ninja
+      - name: Upload artifacts
+        uses: actions/upload-artifact@v2-preview
+        with:
+          name: ${{ env.BUILD_NAME }}_SITL.zip
+          path: ./build_SITL/*_SITL
+
   test:
     needs: [build]
     runs-on: ubuntu-latest

CMakeLists.txt

@@ -44,9 +44,9 @@ else()
         include("${CMAKE_CURRENT_SOURCE_DIR}/cmake/${TOOLCHAIN}.cmake")
     else()
         set(CMAKE_TOOLCHAIN_FILE "${CMAKE_CURRENT_SOURCE_DIR}/cmake/${TOOLCHAIN}.cmake")
-    endif()
         include("${CMAKE_CURRENT_SOURCE_DIR}/cmake/${TOOLCHAIN}-checks.cmake")
     endif()
+endif()
 
 project(INAV VERSION 6.0.0)
 

cmake/host-checks.cmake

@@ -1,10 +0,0 @@
-set(gcc_host_req_ver 10)
-
-execute_process(COMMAND ${CMAKE_C_COMPILER} -dumpversion
-    OUTPUT_STRIP_TRAILING_WHITESPACE
-    OUTPUT_VARIABLE version)
-
-    message("-- found GCC ${version}")
-if (NOT gcc_host_req_ver STREQUAL version)
-    message(FATAL_ERROR "-- expecting GCC version ${gcc_host_req_ver}, but got version ${version} instead")
-endif()

docs/SITL/RealFlight.md

@@ -2,10 +2,14 @@
 
 Supported are RealFlight 9.5S and RealFlight Evolution, NOT RealFlight-X.
 
-RealFlight is very well suited to simulate the model flight specific aspects. Autolaunch and the mixers can be set in INAV.
+RealFlight is very well suited to simulate the model flight specific aspects. Autolaunch and the mixers can be used.
 However, since the sceneries do not correspond to a real environment, the GPS data must be "faked". The position is always shown somewhere in southern Nevada ;).
 GPS data and flight modes work fine though, only for missions with waypoints it is of course not ideal. 
 
+## Joystick 
+In the settings, calibrate the joystick, set it up and assign the axes in the same order as in INAV.
+Channel 1 (Aileron) in RealFlight is Cannel 1 (Aileron in INAV) and so on. 
+
 ## General settings
 Under Settings / Physics / Quality Switch on "RealFlight Link enabled".
 As a command line option for SITL, the port does not need to be specified, the port is fixed.
@@ -17,4 +21,4 @@ In the model editor under "Electronis" all mixers should be deleted and the serv
 In the "Radio" tab also deactivate Expo and low rates: "Activadd when: Never".
 Configure the model in the same way as a real model would be set up in INAV including Mixer, Expo, etc. depending on the selected model in RealFlight.
 
-Then adjust the channelmap (see command line option) accordingly. 
+Then adjust the channelmap im the Configurator or via command line accordingly. 

docs/SITL/SITL.md

@@ -3,7 +3,7 @@
 ![INAV-SIM-OSD](assets/INAV-SIM-OSD.png)
 
 ## ATTENTION!
-SITL is currently still under development, its use is still a bit cumbersome. 
+SITL is currently still under development.
 
 SITL (Software in the loop) allows to run INAV completely in software on the PC without using a flight controller and simulate complete FPV flights.
 For this, INAV is compiled with a normal PC compiler.
@@ -13,46 +13,23 @@ Currently supported are
 - RealFlight  https://www.realflight.com/
 - X-Plane https://www.x-plane.com/
 
-INAV SITL communicates for sensor data and control directly with the corresponding simulator, see the documentation of the individual simulators and the command line options.
+INAV SITL communicates for sensor data and control directly with the corresponding simulator, see the documentation of the individual simulators and the Configurator or the command line options.
-
-## Command line
-The following SITL specific command line options are available:
-
-If SITL is started without command line options, only the Configurator can be used.
-
-```--help``` Displays help for the command line options.
-
-```--sim=[sim]``` Select the simulator. xp = X-Plane, rf = RealFlight. Example: ```--sim=xp```
-
-```--simip=[ip]``` IP address of the simulator, if you specify a simulator with "--sim" and omit this option localhost (127.0.0.1) will be used. Example: ```--simip=172.65.21.15```
-
-```--simport=[port]``` Port number of the simulator, not necessary for all simulators. Example: ```--simport=4900```
-
-```--useimu``` Use IMU sensor data from the simulator instead of using attitude data directly from the simulator. Not recommended, use only for debugging.
-
-```--chanmap=[chanmap]``` The channelmap to map the motor and servo outputs from INAV to the virtual receiver channel or control surfaces around simulator.
-Syntax: (M(otor)|S(ervo)<INAV-OUT>-<RECEIVER_OUT>),..., all numbers must have two digits.
-
-Example:
-To assign motor1 to virtual receiver channel 1, servo 1 to channel 2, and servo2 to channel 3:
-```--chanmap:M01-01,S01-02,S02-03```
-Please also read the documentation of the individual simulators.
 
 ## Sensors
 The following sensors are emulated:
 - IMU (Gyro, Accelerometer)
 - GPS
 - Pitot
-- barometer
+- Magnetometer (Compass)
+- Rangefinder
+- Barometer
 - Battery (current and voltage), depending on simulator
 
 ![SITL-Fake-Sensors](assets/SITL-Fake-Sensors.png)
 
-Magnetometer (compass) is not yet supported, therefore no support for copters.
-
 Select "FAKE" as type for all mentioned, so that they receive the data from the simulator. 
 
-## Serial ports
+## Serial ports+
 UARTs are replaced by TCP starting with port 5760 ascending. UART 1 port 5760, UART2 6761, ...
 By default, UART1 and UART2 are available as MSP connections. 
 To connect the Configurator to SITL: Select TCP and connect to ```127.0.0.1:5760``` (if SITL is running on the same machine).
@@ -64,8 +41,17 @@ The assignment and status of user UART/TCP connections is displayed on the conso
 All other interfaces (I2C, SPI, etc.) are not emulated.
 
 ## Remote control
-At the moment only direct input via UART/TCP is supported. 
+Joystick (via simulator) or serial receiver via USB/Serial interface are supported. 
+
+### Joystick interface
+Only 8 channels are supported. 
+Select "SIM (SITL)" as the receiver and set up a joystick in the simulator, details of which can be found in the documentation for the individual simulators.  
+
+### Serial Receiver via USB
 Connect a serial receiver (e.g. SBUS) to the PC via a UART/USB adapter. Configure the receiver in the Configurator as usual.
+
+The Configurator offers a built-in option for forwarding the serial data to the SITL TCP port, if SITL is started manually the following option can be used:
+
 The connection can then be established with a programme that forwards the serial data unaltered to TCP, e.g. with the Python script tcp_serial_redirect.py (https://github.com/Scavanger/TCP-Serial-Redirect)
 If necessary, please download the required runtime environment from https://www.python.org/.
 Please use the linked version, which has a smaller buffer, otherwise the control response is relatively slow.
@@ -78,7 +64,7 @@ For this you need a FT232 module. With FT-Prog (https://ftdichip.com/utilities/)
 For SBUS, the command line arguments of the python script are: 
 ```python tcp_serial_redirect.py --parity E --stopbits 2 -c 127.0.0.1:[INAV-UART-PORT] COMXX 100000``` 
 
-Note: Telemetry via return channel through the receiver is not supported by SITL.
+Note: Telemetry via return channel through the receiver is not supported by SITL (yet).
 
 ## OSD
 For the OSD the program INAV-Sim-OSD is available: https://github.com/Scavanger/INAV-SIM-OSD.
@@ -86,6 +72,33 @@ For this, activate MSP-Displayport on a UART/TCP port and connect to the corresp
 
 Note: INAV-Sim-OSD only works if the simulator is in window mode. 
 
+## Command line
+The command line options are only necessary if the SITL executable is started by hand, e.g. when debugging.
+For normal use, please use the SITL tab in the configurator. 
+
+The following SITL specific command line options are available:
+
+If SITL is started without command line options, only the Configurator can be used.
+
+```--path``` Full path and file name to config file, if not present, eeprom.bin in the current directory is used. Example: ```C:\INAV_SITL\flying-wing.bin```
+
+```--sim=[sim]``` Select the simulator. xp = X-Plane, rf = RealFlight. Example: ```--sim=xp```
+
+```--simip=[ip]``` IP address of the simulator, if you specify a simulator with "--sim" and omit this option localhost (127.0.0.1) will be used. Example: ```--simip=172.65.21.15```
+
+```--simport=[port]``` Port number of the simulator, not necessary for all simulators. Example: ```--simport=4900```
+
+```--useimu``` Use IMU sensor data from the simulator instead of using attitude data directly from the simulator. Not recommended, use only for debugging.
+
+```--chanmap=[chanmap]``` The channelmap to map the motor and servo outputs from INAV to the virtual receiver channel or control surfaces around simulator.
+Syntax: (M(otor)|S(ervo)<INAV-OUT>-<RECEIVER_OUT>),..., all numbers must have two digits.
+Example:
+To assign motor1 to virtual receiver channel 1, servo 1 to channel 2, and servo2 to channel 3:
+```--chanmap:M01-01,S01-02,S02-03```
+Please also read the documentation of the individual simulators.
+
+```--help``` Displays help for the command line options.
+
 ## Running SITL
 It is recommended to start the tools in the following order:
 1. Simulator, aircraft should be ready for take-off
@@ -94,7 +107,6 @@ It is recommended to start the tools in the following order:
 4. serial redirect for RC input
 
 ## Compile
-GCC 10 is required, GCC 11 gives an error message (bug in GCC?!?).
 
 ### Linux:
 Almost like normal, ruby, cmake and make are also required.

docs/SITL/X-Plane.md

@@ -2,14 +2,32 @@
 
 Tested on X-Plane 11, 12 should(!) work but not tested.
 
-X-Plane is not a model flight simulator, but is based on real world data and is therefore suitable 
+X-Plane is not a model flight simulator, but is based on real world data and is therefore suitable for GPS missions with waypoints.
-GPS missions with waypoints.
+
+## Aircraft
+It is recommended to use the "AR Wing" of the INAV HITL project: https://github.com/RomanLut/INAV-X-Plane-HITL
 
 ## General settings
 In Settings / Network select "Accept incoming connections".
 The port can be found under "UDP PORTS", "Port we receive on". If no connection is established, the port can be changed.
 You may want to incease the "Flight model per frame" value under "General"
 
+## Joystick
+In the settings, calibrate the joystick, set it up and assign the axes as follows:
+
+| INAV | X-Plane |
+|------|---------|
+| Roll | Roll |
+| Pitch | Pitch |
+| Throttle | Throttle |
+| Yaw | Yaw | 
+| Channel 5 | Prop | 
+| Channel 6 | Mixture | 
+| Channel 7 | Collective | 
+| Channel 8 | Thrust vector | 
+
+Reverse axis in X-Plane if necessary.
+
 ## Channelmap:
 The assignment of the "virtual receiver" is fixed: 
 1 - Throttle
@@ -17,6 +35,6 @@ The assignment of the "virtual receiver" is fixed:
 3 - Pitch
 4 - Yaw
 
-The internal mixer (e.g. for wings only) cannot be deactivated without further ado, therefore always select "Aircraft with tail" in INAV. 
+The internal mixer (e.g. for flying wings) cannot be deactivated without further ado, therefore always select "Aircraft with tail" in INAV. 
 For the standard Aircraft preset the channelmap is:
 ```--chanmap=M01-01,S01-03,S03-02,S04-04```

src/main/CMakeLists.txt

@@ -406,6 +406,9 @@ main_sources(COMMON_SRC
     rx/srxl2.h
     rx/sumd.c
     rx/sumd.h
+    rx/sim.c
+    rx/sim.h
+
 
     scheduler/scheduler.c
     scheduler/scheduler.h
@@ -467,6 +470,7 @@ main_sources(COMMON_SRC
     io/rangefinder.h
     io/rangefinder_msp.c
     io/rangefinder_benewake.c
+    io/rangefinder_fake.c
     io/opflow.h
     io/opflow_cxof.c
     io/opflow_msp.c

src/main/common/time.c

@@ -35,6 +35,10 @@
 
 #include "fc/settings.h"
 
+#ifdef SITL_BUILD
+#include <time.h>
+#endif
+
 // For the "modulo 4" arithmetic to work, we need a leap base year
 #define REFERENCE_YEAR 2000
 // Offset (seconds) from the UNIX epoch (1970-01-01) to 2000-01-01
@@ -310,11 +314,16 @@ bool rtcHasTime(void)
 
 bool rtcGet(rtcTime_t *t)
 {
+#ifdef SITL_BUILD
+    *t = (rtcTime_t)(time(NULL) * 1000);
+    return true;
+#else 
     if (!rtcHasTime()) {
         return false;
     }
     *t = started + millis();
     return true;
+#endif
 }
 
 bool rtcSet(rtcTime_t *t)
@@ -323,6 +332,7 @@ bool rtcSet(rtcTime_t *t)
     return true;
 }
 
+
 bool rtcGetDateTime(dateTime_t *dt)
 {
     rtcTime_t t;

src/main/config/config_streamer.h

@@ -57,3 +57,7 @@ int config_streamer_flush(config_streamer_t *c);
 
 int config_streamer_finish(config_streamer_t *c);
 int config_streamer_status(config_streamer_t *c);
+
+#if defined(CONFIG_IN_FILE)
+bool configFileSetPath(char* path);
+#endif

src/main/config/config_streamer_file.c

@@ -16,6 +16,7 @@
  */
 
 #include <string.h>
+#include <dirent.h>
 #include "platform.h"
 #include "drivers/system.h"
 #include "config/config_streamer.h"
@@ -26,21 +27,32 @@
 #include <stdio.h>
 #include <errno.h>
 
+
 #define FLASH_PAGE_SIZE  (0x400)
 
 static FILE *eepromFd = NULL;
-
 static bool streamerLocked = true;
+static char eepromPath[260] = EEPROM_FILENAME;
+
+bool configFileSetPath(char* path)
+{
+    if(!path || strlen(path) > 260) {
+        return false;
+    }
+    
+    strcpy(eepromPath, path);
+    return true;
+}
 
 void config_streamer_impl_unlock(void)
 {
     if (eepromFd != NULL) {
-        fprintf(stderr, "[EEPROM] Unable to load %s\n", EEPROM_FILENAME);
+        fprintf(stderr, "[EEPROM] Unable to load %s\n", eepromPath);
         return;
     }
 
     // open or create
-    eepromFd = fopen(EEPROM_FILENAME,"r+");
+    eepromFd = fopen(eepromPath,"r+");
     if (eepromFd != NULL) {
         // obtain file size:
         fseek(eepromFd , 0 , SEEK_END);
@@ -49,16 +61,16 @@ void config_streamer_impl_unlock(void)
 
         size_t n = fread(eepromData, 1, sizeof(eepromData), eepromFd);
         if (n == size) {
-            printf("[EEPROM] Loaded '%s' (%ld of %ld bytes)\n", EEPROM_FILENAME, size, sizeof(eepromData));
+            fprintf(stderr,"[EEPROM] Loaded '%s' (%ld of %ld bytes)\n", eepromPath, size, sizeof(eepromData));
             streamerLocked = false;
         } else {
-            fprintf(stderr, "[EEPROM] Failed to load '%s'\n", EEPROM_FILENAME);
+            fprintf(stderr, "[EEPROM] Failed to load '%s'\n", eepromPath);
         }
     } else {
-        printf("[EEPROM] Created '%s', size = %ld\n", EEPROM_FILENAME, sizeof(eepromData));
+        printf("[EEPROM] Created '%s', size = %ld\n", eepromPath, sizeof(eepromData));
         streamerLocked = false;
-        if ((eepromFd = fopen(EEPROM_FILENAME, "w+")) == NULL) {
+        if ((eepromFd = fopen(eepromPath, "w+")) == NULL) {
-            fprintf(stderr, "[EEPROM] Failed to create '%s'\n", EEPROM_FILENAME);
+            fprintf(stderr, "[EEPROM] Failed to create '%s'\n", eepromPath);
             streamerLocked = true;
         }
         if (fwrite(eepromData, sizeof(eepromData), 1, eepromFd) != 1) {
@@ -66,8 +78,6 @@ void config_streamer_impl_unlock(void)
             streamerLocked = true;
         }
     }
-    
-    
 }
 
 void config_streamer_impl_lock(void)
@@ -78,7 +88,7 @@ void config_streamer_impl_lock(void)
         fwrite(eepromData, 1, sizeof(eepromData), eepromFd);
         fclose(eepromFd);
         eepromFd = NULL;
-        printf("[EEPROM] Saved '%s'\n", EEPROM_FILENAME);
+        fprintf(stderr, "[EEPROM] Saved '%s'\n", eepromPath);
         streamerLocked = false;
     } else {
         fprintf(stderr, "[EEPROM] Unlock error\n");
@@ -93,9 +103,9 @@ 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;
-        printf("[EEPROM] Program word  %p = %08x\n", (void*)c->address, *((uint32_t*)c->address));
+        fprintf(stderr, "[EEPROM] Program word  %p = %08x\n", (void*)c->address, *((uint32_t*)c->address));
     } else {
-        printf("[EEPROM] Program word %p out of range!\n", (void*)c->address);
+        fprintf(stderr, "[EEPROM] Program word %p out of range!\n", (void*)c->address);
     }
 
     c->address += CONFIG_STREAMER_BUFFER_SIZE;

src/main/drivers/accgyro/accgyro_fake.c

@@ -32,59 +32,25 @@
 
 #ifdef USE_IMU_FAKE
 
-#if defined(SITL_BUILD)
+static int16_t fakeGyroADC[XYZ_AXIS_COUNT];
-
-#define VOLATILE volatile
-
-static pthread_mutex_t gyroMutex;
-static pthread_mutex_t accMutex;
-
-#define LOCK(mutex) (pthread_mutex_lock(mutex))
-#define UNLOCK(mutex) (pthread_mutex_unlock(mutex))
-
-#define GYROLOCK (pthread_mutex_lock(&gyroMutex))
-#define GYROUNLOCK (pthread_mutex_unlock(&gyroMutex))
-#define ACCLOCK (pthread_mutex_lock(&accMutex))
-#define ACCUNLOCK (pthread_mutex_unlock(&accMutex))
-
-#else
-#define VOLATILE
-#define GYROLOCK
-#define GYROUNLOCK
-#define ACCLOCK
-#define ACCUNLOCK
-
-#endif
-
-static VOLATILE int16_t fakeGyroADC[XYZ_AXIS_COUNT];
 
 static void fakeGyroInit(gyroDev_t *gyro)
 {
     UNUSED(gyro);
-
-#if defined(SITL_BUILD)
-    pthread_mutex_init(&gyroMutex, NULL);
-#endif
-
-    //ENABLE_STATE(ACCELEROMETER_CALIBRATED);
 }
 
 void fakeGyroSet(int16_t x, int16_t y, int16_t z)
 {
-    GYROLOCK;
     fakeGyroADC[X] = x;
     fakeGyroADC[Y] = y;
     fakeGyroADC[Z] = z;
-    GYROUNLOCK;
 }
 
 static bool fakeGyroRead(gyroDev_t *gyro)
 {
-    GYROLOCK;
     gyro->gyroADCRaw[X] = fakeGyroADC[X];
     gyro->gyroADCRaw[Y] = fakeGyroADC[Y];
     gyro->gyroADCRaw[Z] = fakeGyroADC[Z];
-    GYROUNLOCK;
     return true;
 }
 
@@ -112,33 +78,25 @@ bool fakeGyroDetect(gyroDev_t *gyro)
     return true;
 }
 
-static VOLATILE int16_t fakeAccData[XYZ_AXIS_COUNT];
+static int16_t fakeAccData[XYZ_AXIS_COUNT];
 
 static void fakeAccInit(accDev_t *acc)
 {
-#if defined(SITL_BUILD)
-    pthread_mutex_init(&accMutex, NULL);
-#endif
-    
     acc->acc_1G = 9806;
 }
 
 void fakeAccSet(int16_t x, int16_t y, int16_t z)
 { 
-    ACCLOCK;
     fakeAccData[X] = x;
     fakeAccData[Y] = y;
     fakeAccData[Z] = z;
-    ACCUNLOCK;
 }
 
 static bool fakeAccRead(accDev_t *acc)
 {
-    ACCLOCK;
     acc->ADCRaw[X] = fakeAccData[X];
     acc->ADCRaw[Y] = fakeAccData[Y];
     acc->ADCRaw[Z] = fakeAccData[Z];
-    ACCUNLOCK;
     return true;
 }
 

src/main/drivers/rangefinder/rangefinder_virtual.c

@@ -58,18 +58,15 @@ static int32_t virtualRangefinderGetDistance(rangefinderDev_t * dev)
     return highLevelDeviceVTable->read();
 }
 
-#define VIRTUAL_MAX_RANGE_CM                250
-#define VIRTUAL_DETECTION_CONE_DECIDEGREES  900
-
 bool virtualRangefinderDetect(rangefinderDev_t * dev, const virtualRangefinderVTable_t * vtable)
 {
     if (vtable && vtable->detect()) {
         highLevelDeviceVTable = vtable;
 
         dev->delayMs = RANGEFINDER_VIRTUAL_TASK_PERIOD_MS;
-        dev->maxRangeCm = VIRTUAL_MAX_RANGE_CM;
+        dev->maxRangeCm = RANGEFINDER_VIRTUAL_MAX_RANGE_CM;
-        dev->detectionConeDeciDegrees = VIRTUAL_DETECTION_CONE_DECIDEGREES;
+        dev->detectionConeDeciDegrees = RANGEFINDER_VIRTUAL_DETECTION_CONE_DECIDEGREES;
-        dev->detectionConeExtendedDeciDegrees = VIRTUAL_DETECTION_CONE_DECIDEGREES;
+        dev->detectionConeExtendedDeciDegrees = RANGEFINDER_VIRTUAL_DETECTION_CONE_DECIDEGREES;
 
         dev->init = &virtualRangefinderInit;
         dev->update = &virtualRangefinderUpdate;

src/main/drivers/rangefinder/rangefinder_virtual.h

@@ -26,6 +26,8 @@
 
 #include "drivers/rangefinder/rangefinder.h"
 
+#define RANGEFINDER_VIRTUAL_MAX_RANGE_CM                250
+#define RANGEFINDER_VIRTUAL_DETECTION_CONE_DECIDEGREES  900
 #define RANGEFINDER_VIRTUAL_TASK_PERIOD_MS  30
 
 typedef struct virtualRangefinderVTable_s {

src/main/fc/cli.c

@@ -3417,7 +3417,11 @@ static void cliStatus(char *cmdline)
     cliPrint("OSD: ");
 #if defined(USE_OSD)
     displayPort_t *osdDisplayPort = osdGetDisplayPort();
+    if (osdDisplayPort) {
         cliPrintf("%s [%u x %u]", osdDisplayPort->displayPortType, osdDisplayPort->cols, osdDisplayPort->rows);
+    } else {
+        cliPrintf("no OSD detected");
+    }
 #else 
     cliPrint("not used");
 #endif

src/main/fc/fc_core.c

@@ -161,11 +161,22 @@ bool areSensorsCalibrating(void)
 
 int16_t getAxisRcCommand(int16_t rawData, int16_t rate, int16_t deadband)
 {
-    int16_t stickDeflection;
+    int16_t stickDeflection = 0;
 
+#if defined(SITL_BUILD) // Workaround due to strange bug in GCC > 10.2 https://gcc.gnu.org/bugzilla/show_bug.cgi?id=108914   
+    const int16_t value = rawData - PWM_RANGE_MIDDLE;
+    if (value < -500) {
+        stickDeflection = -500;
+    } else if (value > 500) {
+        stickDeflection = 500;
+    } else {
+        stickDeflection = value;
+    }
+#else
     stickDeflection = constrain(rawData - PWM_RANGE_MIDDLE, -500, 500);   
-    stickDeflection = applyDeadbandRescaled(stickDeflection, deadband, -500, 500);
+#endif
     
+    stickDeflection = applyDeadbandRescaled(stickDeflection, deadband, -500, 500);
     return rcLookup(stickDeflection, rate);
 }
 

src/main/fc/fc_msp.c

@@ -46,11 +46,7 @@
 #include "drivers/compass/compass_msp.h"
 #include "drivers/barometer/barometer_msp.h"
 #include "drivers/pitotmeter/pitotmeter_msp.h"
-#include "drivers/accgyro/accgyro_fake.h"
-#include "drivers/barometer/barometer_fake.h"
-#include "drivers/compass/compass_fake.h"
 #include "sensors/battery_sensor_fake.h"
-#include "drivers/pitotmeter/pitotmeter_fake.h" 
 #include "drivers/bus_i2c.h"
 #include "drivers/display.h"
 #include "drivers/flash.h"

src/main/fc/settings.yaml

@@ -7,7 +7,7 @@ tables:
     values: ["NONE", "AUTO", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "BMI088", "ICM42605", "BMI270", "FAKE"]
     enum: accelerationSensor_e
   - name: rangefinder_hardware
-    values: ["NONE", "SRF10", "VL53L0X", "MSP", "BENEWAKE", "VL53L1X", "US42", "TOF10120_I2C"]
+    values: ["NONE", "SRF10", "VL53L0X", "MSP", "BENEWAKE", "VL53L1X", "US42", "TOF10120_I2C", "FAKE"]
     enum: rangefinderType_e
   - name: mag_hardware
     values: ["NONE", "AUTO", "HMC5883", "AK8975", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "LIS3MDL", "MSP", "RM3100", "VCM5883", "MLX90393", "FAKE"]
@@ -22,7 +22,7 @@ tables:
     values: ["NONE", "AUTO", "MS4525", "ADC", "VIRTUAL", "FAKE", "MSP"]
     enum: pitotSensor_e
   - name: receiver_type
-    values: ["NONE", "SERIAL", "MSP"]
+    values: ["NONE", "SERIAL", "MSP", "SIM (SITL)"]
     enum: rxReceiverType_e
   - name: serial_rx
     values: ["SPEK1024", "SPEK2048", "SBUS", "SUMD", "IBUS", "JETIEXBUS", "CRSF", "FPORT", "SBUS_FAST", "FPORT2", "SRXL2", "GHST", "MAVLINK"]

src/main/io/rangefinder.h

@@ -31,5 +31,7 @@
 
 extern virtualRangefinderVTable_t rangefinderMSPVtable;
 extern virtualRangefinderVTable_t rangefinderBenewakeVtable;
+extern virtualRangefinderVTable_t rangefinderFakeVtable;
 
 void mspRangefinderReceiveNewData(uint8_t * bufferPtr);
+void fakeRangefindersSetData(int32_t data);

src/main/io/rangefinder_fake.c

@@ -0,0 +1,86 @@
+/*
+ * 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/.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <ctype.h>
+#include <math.h>
+
+#include "platform.h"
+
+#include "build/build_config.h"
+#include "build/debug.h"
+
+#include "common/maths.h"
+
+#include "io/serial.h"
+
+#if defined(USE_RANGEFINDER_FAKE)
+#include "drivers/rangefinder/rangefinder_virtual.h"
+#include "drivers/time.h"
+#include "io/rangefinder.h"
+
+static bool hasNewData = false;
+static int32_t sensorData = RANGEFINDER_NO_NEW_DATA;
+
+static bool fakeRangefinderDetect(void)
+{
+    // Always detectable
+    return true;
+}
+
+static void fakeRangefinderInit(void)
+{
+}
+
+static void fakeRangefinderUpdate(void)
+{
+}
+
+static int32_t fakeRangefinderGetDistance(void)
+{
+    if (hasNewData) {
+        hasNewData = false;
+        return (sensorData > 0) ? sensorData : RANGEFINDER_OUT_OF_RANGE;
+    }
+    else {
+        return RANGEFINDER_NO_NEW_DATA;
+    }
+}
+
+void fakeRangefindersSetData(int32_t data)
+{
+    sensorData = data;
+    hasNewData = true;
+}
+
+virtualRangefinderVTable_t rangefinderFakeVtable = {
+    .detect = fakeRangefinderDetect,
+    .init = fakeRangefinderInit,
+    .update = fakeRangefinderUpdate,
+    .read = fakeRangefinderGetDistance
+};
+
+
+#endif

src/main/navigation/navigation_pos_estimator.c

@@ -241,7 +241,7 @@ void onNewGPSData(void)
 
                 /* Use VELNED provided by GPS if available, calculate from coordinates otherwise */
                 float gpsScaleLonDown = constrainf(cos_approx((ABS(gpsSol.llh.lat) / 10000000.0f) * 0.0174532925f), 0.01f, 1.0f);
-                if (positionEstimationConfig()->use_gps_velned && gpsSol.flags.validVelNE) {
+                if (!ARMING_FLAG(SIMULATOR_MODE_SITL) && positionEstimationConfig()->use_gps_velned && gpsSol.flags.validVelNE) {
                     posEstimator.gps.vel.x = gpsSol.velNED[X];
                     posEstimator.gps.vel.y = gpsSol.velNED[Y];
                 }

src/main/rx/rx.c

@@ -63,6 +63,7 @@
 #include "rx/sumd.h"
 #include "rx/ghst.h"
 #include "rx/mavlink.h"
+#include "rx/sim.h"
 
 const char rcChannelLetters[] = "AERT";
 
@@ -310,6 +311,12 @@ void rxInit(void)
             break;
 #endif
 
+#ifdef USE_RX_SIM
+    case RX_TYPE_SIM:
+        rxSimInit(rxConfig(), &rxRuntimeConfig);
+        break;
+#endif
+
         default:
         case RX_TYPE_NONE:
             rxConfigMutable()->receiverType = RX_TYPE_NONE;

src/main/rx/rx.h

@@ -62,7 +62,8 @@ typedef enum {
 typedef enum {
     RX_TYPE_NONE = 0,
     RX_TYPE_SERIAL,
-    RX_TYPE_MSP
+    RX_TYPE_MSP,
+    RX_TYPE_SIM
 } rxReceiverType_e;
 
 typedef enum {

src/main/rx/sim.c

@@ -0,0 +1,71 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it 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.
+ *
+ * Cleanflight 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 Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "platform.h"
+
+#ifdef USE_RX_SIM
+
+#include "build/build_config.h"
+
+#include "common/utils.h"
+
+#include "rx/rx.h"
+#include "rx/sim.h"
+
+static uint16_t channels[MAX_SUPPORTED_RC_CHANNEL_COUNT];
+static bool hasNewData = false;
+
+static uint16_t rxSimReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfigPtr, uint8_t chan)
+{
+    UNUSED(rxRuntimeConfigPtr);
+    return channels[chan];
+}
+
+void rxSimSetChannelValue(uint16_t* values, uint8_t count)
+{
+    for (size_t i = 0; i < count; i++) {    
+        channels[i] = values[i];
+    }
+
+    hasNewData = true;
+}
+
+static uint8_t rxSimFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
+{
+    UNUSED(rxRuntimeConfig);
+    
+    if (!hasNewData) {    
+        return RX_FRAME_PENDING;
+    }
+
+    hasNewData = false;
+    return RX_FRAME_COMPLETE;
+}
+
+void rxSimInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
+{
+    UNUSED(rxConfig);
+
+    rxRuntimeConfig->channelCount = MAX_SUPPORTED_RC_CHANNEL_COUNT;
+    rxRuntimeConfig->rxSignalTimeout = DELAY_5_HZ;
+    rxRuntimeConfig->rcReadRawFn = rxSimReadRawRC;
+    rxRuntimeConfig->rcFrameStatusFn = rxSimFrameStatus;
+}
+#endif

src/main/rx/sim.h

@@ -0,0 +1,23 @@
+/*
+ * This file is part of INAV.
+ *
+ * Cleanflight is free software: you can redistribute it 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.
+ *
+ * Cleanflight 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 Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "rx/rx.h"
+
+void rxSimSetChannelValue(uint16_t* values, uint8_t count);
+void rxSimInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig);

src/main/sensors/acceleration.c

@@ -520,11 +520,6 @@ void accUpdate(void)
 
     if (!ARMING_FLAG(SIMULATOR_MODE_SITL)) {
         performAcclerationCalibration();
-
-        applyAccelerationZero(&accelerometerConfig()->accZero, &accelerometerConfig()->accGain);  
-        applyAccelerationZero(&accelerometerConfig()->accZero, &accelerometerConfig()->accGain);
-        applyAccelerationZero(&accelerometerConfig()->accZero, &accelerometerConfig()->accGain);  
-        applyAccelerationZero(&accelerometerConfig()->accZero, &accelerometerConfig()->accGain);
         applyAccelerationZero(&accelerometerConfig()->accZero, &accelerometerConfig()->accGain);  
     } 
 

src/main/sensors/rangefinder.c

@@ -141,7 +141,14 @@ static bool rangefinderDetect(rangefinderDev_t * dev, uint8_t rangefinderHardwar
             }
 #endif
             break;
-
+            case RANGEFINDER_FAKE:
+#if defined(USE_RANGEFINDER_FAKE)
+            if(virtualRangefinderDetect(dev, &rangefinderFakeVtable)) {
+                rangefinderHardware = RANGEFINDER_FAKE;
+                rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_VIRTUAL_TASK_PERIOD_MS));
+            }
+#endif
+            break;
             case RANGEFINDER_NONE:
             rangefinderHardware = RANGEFINDER_NONE;
             break;

src/main/sensors/rangefinder.h

@@ -30,6 +30,7 @@ typedef enum {
     RANGEFINDER_VL53L1X     = 5,
     RANGEFINDER_US42        = 6,
     RANGEFINDER_TOF10102I2C = 7,
+    RANGEFINDER_FAKE        = 8,
 } rangefinderType_e;
 
 typedef struct rangefinderConfig_s {

src/main/target/SITL/sim/realFlight.c

@@ -45,14 +45,19 @@
 #include "sensors/acceleration.h"
 #include "sensors/barometer.h"
 #include "drivers/pitotmeter/pitotmeter_fake.h"
+#include "drivers/compass/compass_fake.h"
+#include "drivers/rangefinder/rangefinder_virtual.h"
+#include "io/rangefinder.h"
 #include "common/utils.h"
 #include "common/maths.h"
 #include "flight/mixer.h"
 #include "flight/servos.h"
 #include "flight/imu.h"
 #include "io/gps.h"
+#include "rx/sim.h"
 
 #define RF_PORT 18083
+#define RF_MAX_CHANNEL_COUNT 12
 // RealFlight scenerys doesn't represent real landscapes, so fake some nice coords: Area 51 ;)
 #define FAKE_LAT 37.277127f
 #define FAKE_LON -115.799669f
@@ -212,6 +217,33 @@ static char* getStringFromResponse(const char* response, const char* elementName
     return ret;
 }
 
+static bool getChannelValues(const char* response, uint16_t* channelValues)
+{
+    if (!response) {
+        return false;
+    }
+    
+    const char* channelValueTag = "<m-channelValues-0to1 xsi:type=\"SOAP-ENC:Array\" SOAP-ENC:arrayType=\"xsd:double[12]\">";
+    char* pos = strstr(response, channelValueTag);
+    if (!pos){
+        return false;
+    }
+
+    pos += strlen(channelValueTag);
+    for (size_t i = 0; i < RF_MAX_CHANNEL_COUNT; i++) {
+        char* end = strstr(pos, "</");
+        if (!end) {
+            return false;
+        }
+
+        channelValues[i] = FLOAT_0_1_TO_PWM((float)atof(pos + 6));
+        pos = end + 7;   
+    }
+
+    return true;
+}
+
+
 static void fakeCoords(double posX, double posY, double distanceX, double distanceY, double *lat, double *lon)
 {
     double m = 1 / (2 * (double)M_PIf / 360 * EARTH_RADIUS) / 1000;
@@ -224,7 +256,6 @@ static double convertAzimuth(double azimuth)
     if (azimuth < 0) {
         azimuth += 360;
     }
-
     return 360 - fmod(azimuth + 90, 360.0f);
 }
 
@@ -246,8 +277,8 @@ static void exchangeData(void)
     //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");
-    rfValues.m_altitudeASL_MTR = getDoubleFromResponse(response, "<m-altitudeASL-MTR");
+    rfValues.m_altitudeASL_MTR = getDoubleFromResponse(response, "m-altitudeASL-MTR");
-    //rfValues.m_altitudeAGL_MTR = getDoubleFromResponse(response, "<m-altitudeAGL-MTR");
+    rfValues.m_altitudeAGL_MTR = getDoubleFromResponse(response, "m-altitudeAGL-MTR");
     rfValues.m_groundspeed_MPS = getDoubleFromResponse(response, "m-groundspeed-MPS");
     rfValues.m_pitchRate_DEGpSEC = getDoubleFromResponse(response, "m-pitchRate-DEGpSEC");
     rfValues.m_rollRate_DEGpSEC = getDoubleFromResponse(response, "m-rollRate-DEGpSEC");
@@ -289,6 +320,11 @@ static void exchangeData(void)
     //rfValues.m_flightAxisControllerIsActive= getBoolFromResponse(response, "m-flightAxisControllerIsActive");
     rfValues.m_currentAircraftStatus = getStringFromResponse(response, "m-currentAircraftStatus");
 
+    
+    uint16_t channelValues[RF_MAX_CHANNEL_COUNT];
+    getChannelValues(response, channelValues);
+    rxSimSetChannelValue(channelValues, RF_MAX_CHANNEL_COUNT);
+    
     double lat, lon;
     fakeCoords(FAKE_LAT, FAKE_LON, rfValues.m_aircraftPositionX_MTR, -rfValues.m_aircraftPositionY_MTR, &lat, &lon);
     
@@ -308,12 +344,18 @@ static void exchangeData(void)
         0
     );
 
+    int32_t altitudeOverGround = (int32_t)round(rfValues.m_altitudeAGL_MTR * 100);
+    if (altitudeOverGround > 0 && altitudeOverGround <= RANGEFINDER_VIRTUAL_MAX_RANGE_CM) {
+        fakeRangefindersSetData(altitudeOverGround);
+    } else {
+        fakeRangefindersSetData(-1);
+    }
+
+    const int16_t roll_inav = (int16_t)round(rfValues.m_roll_DEG * 10);
+    const int16_t pitch_inav = (int16_t)round(-rfValues.m_inclination_DEG * 10);
+    const int16_t yaw_inav = course;
     if (!useImu) {
-        imuSetAttitudeRPY(
+        imuSetAttitudeRPY(roll_inav, pitch_inav, yaw_inav);
-            (int16_t)round(rfValues.m_roll_DEG * 10), 
-            (int16_t)round(-rfValues.m_inclination_DEG * 10), 
-            course
-        );
         imuUpdateAttitude(micros());
     }
 
@@ -326,26 +368,38 @@ static void exchangeData(void)
         accY = 0;
         accZ = (int16_t)(GRAVITY_MSS * 1000.0f);
     } else {
-         accX = clampToInt16(rfValues.m_accelerationBodyAX_MPS2 * 1000);
+         accX = constrainToInt16(rfValues.m_accelerationBodyAX_MPS2 * 1000);
-         accY = clampToInt16(-rfValues.m_accelerationBodyAY_MPS2 * 1000);
+         accY = constrainToInt16(-rfValues.m_accelerationBodyAY_MPS2 * 1000);
-         accZ = clampToInt16(-rfValues.m_accelerationBodyAZ_MPS2 * 1000);
+         accZ = constrainToInt16(-rfValues.m_accelerationBodyAZ_MPS2 * 1000);
     }
 
     fakeAccSet(accX, accY, accZ);
 
     fakeGyroSet(
-        clampToInt16(rfValues.m_rollRate_DEGpSEC * (double)16.0),
+        constrainToInt16(rfValues.m_rollRate_DEGpSEC * (double)16.0),
-        clampToInt16(-rfValues.m_pitchRate_DEGpSEC * (double)16.0),
+        constrainToInt16(-rfValues.m_pitchRate_DEGpSEC * (double)16.0),
-        clampToInt16(rfValues.m_yawRate_DEGpSEC * (double)16.0)
+        constrainToInt16(rfValues.m_yawRate_DEGpSEC * (double)16.0)
     );
 
     fakeBaroSet(altitudeToPressure(altitude), DEGREES_TO_CENTIDEGREES(21));
     fakePitotSetAirspeed(rfValues.m_airspeed_MPS * 100);
 
-    // TODO fakeMag
-
     fakeBattSensorSetVbat((uint16_t)round(rfValues.m_batteryVoltage_VOLTS * 100));
     fakeBattSensorSetAmperage((uint16_t)round(rfValues.m_batteryCurrentDraw_AMPS * 100)); 
+
+    fpQuaternion_t quat;
+    fpVector3_t north;
+    north.x = 1.0f;
+    north.y = 0;
+    north.z = 0;
+    computeQuaternionFromRPY(&quat, roll_inav, pitch_inav, yaw_inav);
+    transformVectorEarthToBody(&north, &quat);
+    fakeMagSet(
+        constrainToInt16(north.x * 16000.0f),
+        constrainToInt16(north.y * 16000.0f),
+        constrainToInt16(north.z * 16000.0f)
+    );
+    ENABLE_STATE(COMPASS_CALIBRATED);
 }
 
 static void* soapWorker(void* arg)
@@ -358,13 +412,13 @@ static void* soapWorker(void* arg)
             endRequest();
             startRequest("InjectUAVControllerInterface", "<InjectUAVControllerInterface><a>1</a><b>2</b></InjectUAVControllerInterface>");
             endRequest();        
-            
             exchangeData();
             ENABLE_ARMING_FLAG(SIMULATOR_MODE_SITL);
+            
             isInitalised = true;  
         }
-        exchangeData();
 
+        exchangeData();
         unlockMainPID();
     }
 
@@ -414,7 +468,7 @@ bool simRealFlightInit(char* ip, uint8_t* mapping, uint8_t mapCount, bool imu)
     // 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);
     }
 
     return true;

src/main/target/SITL/sim/simHelper.c

@@ -26,22 +26,42 @@
 #include <stdio.h>
 #include <math.h>
 
+#include "common/quaternion.h"
+
 #include "target/SITL/sim/simHelper.h"
 
-inline double clampd(double value, double min, double max)
+inline int16_t constrainToInt16(double value)
 {
-    if (value < min) {
+    return (int16_t)round(constrain(value, INT16_MIN, INT16_MAX));
-        value = min;
 }
 
-    if (value > max) {
+// Move to quaternion.h ?
-        value = max;
+void computeQuaternionFromRPY(fpQuaternion_t *quat, int16_t initialRoll, int16_t initialPitch, int16_t initialYaw)
-    }
-
-    return value;
-}
-
-inline int16_t clampToInt16(double value)
 {
-    return (int16_t)round(clampd(value, INT16_MIN, INT16_MAX));
+    if (initialRoll > 1800) initialRoll -= 3600;
+    if (initialPitch > 1800) initialPitch -= 3600;
+    if (initialYaw > 1800) initialYaw -= 3600;
+
+    const float cosRoll = cos_approx(DECIDEGREES_TO_RADIANS(initialRoll) * 0.5f);
+    const float sinRoll = sin_approx(DECIDEGREES_TO_RADIANS(initialRoll) * 0.5f);
+
+    const float cosPitch = cos_approx(DECIDEGREES_TO_RADIANS(initialPitch) * 0.5f);
+    const float sinPitch = sin_approx(DECIDEGREES_TO_RADIANS(initialPitch) * 0.5f);
+
+    const float cosYaw = cos_approx(DECIDEGREES_TO_RADIANS(-initialYaw) * 0.5f);
+    const float sinYaw = sin_approx(DECIDEGREES_TO_RADIANS(-initialYaw) * 0.5f);
+
+    quat->q0 = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw;
+    quat->q1 = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw;
+    quat->q2 = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw;
+    quat->q3 = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw;
+}
+
+void transformVectorEarthToBody(fpVector3_t *v, const fpQuaternion_t *quat)
+{
+    // HACK: This is needed to correctly transform from NED (sensor frame) to NEU (navigation)
+    v->y = -v->y;
+
+    // From earth frame to body frame
+    quaternionRotateVector(v, v, quat);
 }

src/main/target/SITL/sim/simHelper.h

@@ -23,11 +23,15 @@
  */
 
 #include <stdint.h>
+#include "common/maths.h"
+#include "common/quaternion.h"
 
 #define EARTH_RADIUS ((double)6378.137)
-#define DEG2RAD(deg) (deg * (double)M_PIf / (double)180.0)
 #define PWM_TO_FLOAT_0_1(x) (((int)x - 1000) / 1000.0f)
 #define PWM_TO_FLOAT_MINUS_1_1(x) (((int)x - 1500) / 500.0f)
+#define FLOAT_0_1_TO_PWM(x) ((uint16_t)(x * 1000.0f) + 1000.0f)
+#define FLOAT_MINUS_1_1_TO_PWM(x) ((uint16_t)((x + 1.0f) / 2.0f * 1000.0f) + 1000.0f)
 
-double clampd(double value, double min, double max);
+int16_t constrainToInt16(double value);
-int16_t clampToInt16(double value);
+void transformVectorEarthToBody(fpVector3_t *v, const fpQuaternion_t *quat);
+void computeQuaternionFromRPY(fpQuaternion_t *quat, int16_t initialRoll, int16_t initialPitch, int16_t initialYaw);

src/main/target/SITL/sim/simple_soap_client.c

@@ -69,6 +69,8 @@ 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)
@@ -78,11 +80,15 @@ void soapClientSendRequestVa(soap_client_t *client, const char* action, const ch
     }
 
     char* requestBody;  
-    vasprintf(&requestBody, fmt, va);
+    if (vasprintf(&requestBody, fmt, va) < 0) {
+        return;
+    }
 
     char* request;
-    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>",
+    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);
+         action, (unsigned)strlen(requestBody), requestBody) < 0) {
+            return;
+         }
     
     send(client->sockedFd, request, strlen(request), 0);
 }

src/main/target/SITL/sim/xplane.c

@@ -45,14 +45,19 @@
 #include "sensors/battery_sensor_fake.h"
 #include "sensors/acceleration.h"
 #include "drivers/pitotmeter/pitotmeter_fake.h"
+#include "drivers/compass/compass_fake.h"
+#include "drivers/rangefinder/rangefinder_virtual.h"
+#include "io/rangefinder.h"
 #include "common/utils.h"
 #include "common/maths.h"
 #include "flight/mixer.h"
 #include "flight/servos.h"
 #include "flight/imu.h"
 #include "io/gps.h"
+#include "rx/sim.h"
 
 #define XP_PORT 49000
+#define XPLANE_JOYSTICK_AXIS_COUNT 8
 
 
 static uint8_t pwmMapping[XP_MAX_PWM_OUTS];
@@ -67,6 +72,7 @@ static bool useImu = false;
 static float lattitude = 0; 
 static float longitude = 0;
 static float elevation = 0;
+static float agl = 0;
 static float local_vx = 0;
 static float local_vy = 0;
 static float local_vz = 0;
@@ -83,13 +89,27 @@ static float gyro_x = 0;
 static float gyro_y = 0;
 static float gyro_z = 0;
 static float barometer = 0;
+static bool  hasJoystick = false;
+static float joystickRaw[XPLANE_JOYSTICK_AXIS_COUNT];
 
+enum 
+{
+    RF_ASSIGNMENT_PITCH,
+    RF_ASSIGNMENT_ROLL,
+    RF_ASSIGNMENT_YAW,
+    RF_ASSIGNMENT_THROTLE,
+    RF_ASSIGNMENT_PROP,
+    RF_ASSIGNMENT_MIXTURE,
+    RF_ASSIGNMENT_COLLECTIVE,
+    RF_ASSIGNMENT_THRUST_VECTOR
+};
 
 typedef enum 
 {
     DREF_LATITUDE,
     DREF_LONGITUDE,
     DREF_ELEVATION,
+    DREF_AGL,
     DREF_LOCAL_VX,
     DREF_LOCAL_VY,
     DREF_LOCAL_VZ,
@@ -106,7 +126,16 @@ typedef enum
     DREF_POS_Q,
     DREF_POS_R,
     DREF_POS_BARO_CURRENT_INHG,
-    DREF_COUNT
+    DREF_COUNT,
+    DREF_HAS_JOYSTICK,
+    DREF_JOYSTICK_VALUES_ROll,
+    DREF_JOYSTICK_VALUES_PITCH,
+    DREF_JOYSTICK_VALUES_THROTTLE,
+    DREF_JOYSTICK_VALUES_YAW,
+    DREF_JOYSTICK_VALUES_PROP,
+    DREF_JOYSTICK_VALUES_MIXTURE,
+    DREF_JOYSTICK_VALUES_COLLECTIVE,
+    DREF_JOYSTICK_VALUES_THRUST_VECTOR,
 } dref_t;
 
 uint32_t xint2uint32 (uint8_t * buf) 
@@ -168,7 +197,7 @@ static void* listenWorker(void* arg)
             if (y > 2) {
                 break;
             }
-            if (pwmMapping[i] & 0x80) { // Motor)
+            if (pwmMapping[i] & 0x80) { // Motor
                 motorValue = PWM_TO_FLOAT_0_1(motor[pwmMapping[i] & 0x7f]);
             } else {
                 yokeValues[y] = PWM_TO_FLOAT_MINUS_1_1(servo[pwmMapping[i]]);
@@ -184,11 +213,11 @@ static void* listenWorker(void* arg)
         
         recvLen = recvfrom(sockFd, buf, sizeof(buf), 0, (struct sockaddr*)&remoteAddr, &slen);
         if (recvLen < 0 && errno != EWOULDBLOCK) {
-            break;
+            continue;
         } 
 
         if (strncmp((char*)buf, "RREF", 4) != 0) {
-            break;
+            continue;
         }
 
         for (int i = 5; i < recvLen; i += 8) {
@@ -209,6 +238,10 @@ static void* listenWorker(void* arg)
                     elevation = value;
                     break;
                 
+                case DREF_AGL:
+                    agl = value;
+                    break;
+
                 case DREF_LOCAL_VX:
                     local_vx = value;
                     break;
@@ -273,10 +306,45 @@ static void* listenWorker(void* arg)
                     barometer = value;
                     break;
 
+                case DREF_HAS_JOYSTICK:
+                    hasJoystick = value >= 1 ? true : false;
+                    break;
+                
+                case DREF_JOYSTICK_VALUES_ROll: 
+                    joystickRaw[0] = value;
+                    break;
+
+                case DREF_JOYSTICK_VALUES_PITCH: 
+                    joystickRaw[1] = value;
+                    break;
+
+                case DREF_JOYSTICK_VALUES_THROTTLE: 
+                    joystickRaw[2] = value;
+                    break;
+
+                case DREF_JOYSTICK_VALUES_YAW: 
+                    joystickRaw[3] = value;
+                    break;
+
+                case DREF_JOYSTICK_VALUES_PROP: 
+                    joystickRaw[4] = value;
+                    break;
+
+                case DREF_JOYSTICK_VALUES_MIXTURE: 
+                    joystickRaw[5] = value;
+                    break;
+
+                case DREF_JOYSTICK_VALUES_COLLECTIVE: 
+                    joystickRaw[6] = value;
+                    break;
+
+                case DREF_JOYSTICK_VALUES_THRUST_VECTOR: 
+                    joystickRaw[7] = value;
+                    break;
+
                 default:
                     break;
             }
-
         }
 
         if (hpath < 0) {
@@ -287,6 +355,20 @@ static void* listenWorker(void* arg)
             yaw += 3600;
         }
 
+        if (hasJoystick) {
+            uint16_t channelValues[XPLANE_JOYSTICK_AXIS_COUNT];
+            channelValues[0] = FLOAT_MINUS_1_1_TO_PWM(joystickRaw[RF_ASSIGNMENT_ROLL]);
+            channelValues[1] = FLOAT_MINUS_1_1_TO_PWM(joystickRaw[RF_ASSIGNMENT_PITCH]);
+            channelValues[2] = FLOAT_0_1_TO_PWM(joystickRaw[RF_ASSIGNMENT_THROTLE]);
+            channelValues[3] = FLOAT_MINUS_1_1_TO_PWM(joystickRaw[RF_ASSIGNMENT_YAW]);
+            channelValues[4] = FLOAT_0_1_TO_PWM(joystickRaw[RF_ASSIGNMENT_PROP]);
+            channelValues[5] = FLOAT_0_1_TO_PWM(joystickRaw[RF_ASSIGNMENT_MIXTURE]);
+            channelValues[6] = FLOAT_0_1_TO_PWM(joystickRaw[RF_ASSIGNMENT_COLLECTIVE]);
+            channelValues[7] = FLOAT_0_1_TO_PWM(joystickRaw[RF_ASSIGNMENT_THRUST_VECTOR]);
+
+            rxSimSetChannelValue(channelValues, XPLANE_JOYSTICK_AXIS_COUNT);
+        }
+
         gpsFakeSet(
             GPS_FIX_3D,
             16,
@@ -301,25 +383,32 @@ static void* listenWorker(void* arg)
             0
         );
         
+        const int32_t altitideOverGround = (int32_t)round(agl * 100); 
+        if (altitideOverGround > 0 && altitideOverGround <= RANGEFINDER_VIRTUAL_MAX_RANGE_CM) {
+            fakeRangefindersSetData(altitideOverGround);
+        } else {
+            fakeRangefindersSetData(-1);
+        }
+
+        const int16_t roll_inav = roll * 10;
+        const int16_t pitch_inav = -pitch * 10;
+        const int16_t yaw_inav = yaw * 10;
+
         if (!useImu) {
-            imuSetAttitudeRPY(
+            imuSetAttitudeRPY(roll_inav, pitch_inav, yaw_inav);
-                (int16_t)round(roll * 10), 
-                (int16_t)round(-pitch * 10), 
-                (int16_t)round(yaw * 10)
-            );
             imuUpdateAttitude(micros());
         }
         
         fakeAccSet(
-            clampToInt16(-accel_x * GRAVITY_MSS * 1000),
+            constrainToInt16(-accel_x * GRAVITY_MSS * 1000),
-            clampToInt16(accel_y * GRAVITY_MSS * 1000),
+            constrainToInt16(accel_y * GRAVITY_MSS * 1000),
-            clampToInt16(accel_z * GRAVITY_MSS * 1000)
+            constrainToInt16(accel_z * GRAVITY_MSS * 1000)
         );
 
         fakeGyroSet(
-            clampToInt16(gyro_x * 16.0f),
+            constrainToInt16(gyro_x * 16.0f),
-            clampToInt16(-gyro_y * 16.0f),
+            constrainToInt16(-gyro_y * 16.0f),
-            clampToInt16(-gyro_z * 16.0f)
+            constrainToInt16(-gyro_z * 16.0f)
         );
 
         fakeBaroSet((int32_t)round(barometer * 3386.39f), DEGREES_TO_CENTIDEGREES(21));
@@ -327,8 +416,24 @@ static void* listenWorker(void* arg)
 
         fakeBattSensorSetVbat(16.8 * 100);
 
+        fpQuaternion_t quat;
+        fpVector3_t north;
+        north.x = 1.0f;
+        north.y = 0;
+        north.z = 0;
+        computeQuaternionFromRPY(&quat, roll_inav, pitch_inav, yaw_inav);
+        transformVectorEarthToBody(&north, &quat);
+        fakeMagSet(
+            constrainToInt16(north.x * 16000.0f),
+            constrainToInt16(north.y * 16000.0f),
+            constrainToInt16(north.z * 16000.0f)
+        );
+        ENABLE_STATE(COMPASS_CALIBRATED);    
+
         if (!initalized) {
             ENABLE_ARMING_FLAG(SIMULATOR_MODE_SITL);
+            // Aircraft can wobble on the runway and prevents calibration of the accelerometer
+            ENABLE_STATE(ACCELEROMETER_CALIBRATED);                  
             initalized = true;
         }
 
@@ -362,17 +467,25 @@ bool simXPlaneInit(char* ip, int port, uint8_t* mapping, uint8_t mapCount, bool
         return false;
     }
 
-    if (bind(sockFd, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
+    if (setsockopt(sockFd, SOL_SOCKET, SO_SNDTIMEO, (struct timeval *) &tv,sizeof(struct timeval))) {
         return false;
     }
          
+    bind(sockFd, (struct sockaddr *) &addr, sizeof(addr));
+
     serverAddr.sin_family = AF_INET;
 	serverAddr.sin_addr.s_addr = inet_addr(ip);
 	serverAddr.sin_port = htons(port);
     
+    if (pthread_create(&listenThread, NULL, listenWorker, NULL) < 0) {
+        return false;
+    }
+
+    while (!initalized) {
         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);
@@ -389,15 +502,17 @@ bool simXPlaneInit(char* ip, int port, uint8_t* mapping, uint8_t mapCount, bool
         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);
-    if (pthread_create(&listenThread, NULL, listenWorker, NULL) < 0) {
+        registerDref(DREF_JOYSTICK_VALUES_ROll, "sim/joystick/joy_mapped_axis_value[1]", 100);
-        return false;
+        registerDref(DREF_JOYSTICK_VALUES_PITCH, "sim/joystick/joy_mapped_axis_value[2]", 100);
-    }
+        registerDref(DREF_JOYSTICK_VALUES_THROTTLE, "sim/joystick/joy_mapped_axis_value[3]", 100);
-
+        registerDref(DREF_JOYSTICK_VALUES_YAW, "sim/joystick/joy_mapped_axis_value[4]", 100);
-    while(!initalized) {
+        registerDref(DREF_JOYSTICK_VALUES_PROP, "sim/joystick/joy_mapped_axis_value[8]", 100);
-        //
+        registerDref(DREF_JOYSTICK_VALUES_MIXTURE, "sim/joystick/joy_mapped_axis_value[9]", 100);
+        registerDref(DREF_JOYSTICK_VALUES_COLLECTIVE, "sim/joystick/joy_mapped_axis_value[5]", 100);
+        registerDref(DREF_JOYSTICK_VALUES_THRUST_VECTOR, "sim/joystick/joy_mapped_axis_value[12]", 100);
+        delay(250);
     }
 
     return true;
-
 }

src/main/target/SITL/target.c

@@ -33,6 +33,7 @@
 #include <errno.h>
 #include <time.h>
 #include <pthread.h>
+#include <unistd.h>
 
 #include <platform.h>
 #include "target.h"
@@ -44,6 +45,7 @@
 #include "drivers/pwm_mapping.h"
 #include "drivers/timer.h"
 #include "drivers/serial.h"
+#include "config/config_streamer.h"
 
 #include "target/SITL/sim/realFlight.h"
 #include "target/SITL/sim/xplane.h"
@@ -66,46 +68,55 @@ static int simPort = 0;
 
 void systemInit(void) {
 
-    printf("INAV %d.%d.%d SITL\n", FC_VERSION_MAJOR, FC_VERSION_MINOR, FC_VERSION_PATCH_LEVEL);
+    fprintf(stderr, "INAV %d.%d.%d SITL\n", FC_VERSION_MAJOR, FC_VERSION_MINOR, FC_VERSION_PATCH_LEVEL);
     clock_gettime(CLOCK_MONOTONIC, &start_time);
-    printf("[SYSTEM] Init...\n");
+    fprintf(stderr, "[SYSTEM] Init...\n");
+
+    pthread_attr_t thAttr;
+    int policy = 0;
+
+    pthread_attr_init(&thAttr);
+    pthread_attr_getschedpolicy(&thAttr, &policy);
+
+    pthread_setschedprio(pthread_self(), sched_get_priority_min(policy));
+    pthread_attr_destroy(&thAttr);
 
     if (pthread_mutex_init(&mainLoopLock, NULL) != 0) {
-        printf("[SYSTEM] Unable to create mainLoop lock.\n");
+        fprintf(stderr, "[SYSTEM] Unable to create mainLoop lock.\n");
         exit(1);
     }
 
     if (sitlSim != SITL_SIM_NONE) {
-        printf("[SIM] Waiting for connection...\n");
+        fprintf(stderr, "[SIM] Waiting for connection...\n");
     }
     
     switch (sitlSim) {
         case SITL_SIM_REALFLIGHT:
             if (mappingCount > RF_MAX_PWM_OUTS) {
-                printf("[SIM] Mapping error. RealFligt supports a maximum of %i PWM outputs.", RF_MAX_PWM_OUTS);
+                fprintf(stderr, "[SIM] Mapping error. RealFligt supports a maximum of %i PWM outputs.", RF_MAX_PWM_OUTS);
                 sitlSim = SITL_SIM_NONE;
                 break;
             }
             if (simRealFlightInit(simIp, pwmMapping, mappingCount, useImu)) {
-                printf("[SIM] Connection with RealFlight (%s) successfully established. \n", simIp);
+                fprintf(stderr, "[SIM] Connection with RealFlight successfully established.\n");
             } else {
-                printf("[SIM] Connection with RealFlight (%s) NOT established. \n", simIp);
+                fprintf(stderr, "[SIM] Connection with RealFlight NOT established.\n");
             }
             break;
         case SITL_SIM_XPLANE:
             if (mappingCount > XP_MAX_PWM_OUTS) {
-                printf("[SIM] Mapping error. RealFligt supports a maximum of %i PWM outputs.", XP_MAX_PWM_OUTS);
+                fprintf(stderr, "[SIM] Mapping error. RealFligt supports a maximum of %i PWM outputs.", XP_MAX_PWM_OUTS);
                 sitlSim = SITL_SIM_NONE;
                 break;
             }            
             if (simXPlaneInit(simIp, simPort, pwmMapping, mappingCount, useImu)) {
-                printf("[SIM] Connection with XPlane successfully established. \n");
+                fprintf(stderr, "[SIM] Connection with X-Plane successfully established.\n");
             } else {
-                printf("[SIM] Connection with XPLane NOT established. \n");
+                fprintf(stderr, "[SIM] Connection with X-PLane NOT established.\n");
             }
             break;
         default:
-          printf("[SIM] No interface specified. Configurator only.\n");
+          fprintf(stderr, "[SIM] No interface specified. Configurator only.\n");
           break;
     }
  
@@ -149,21 +160,22 @@ bool parseMapping(char* mapStr)
 
 void printCmdLineOptions(void)         
 {
-    printf("Avaiable options:\n");
+    fprintf(stderr, "Avaiable options:\n");
-    printf("--sim=[rf|xp]                        Simulator interface: rf = RealFligt, xp = XPlane. Example: --sim=rf\n");
+    fprintf(stderr, "--path=[path]                        Path and filename of eeprom.bin. If not specified 'eeprom.bin' in program directory is used.\n");
-    printf("--simip=[ip]                         IP-Address oft the simulator host. If not specified localhost (127.0.0.1) is used.");
+    fprintf(stderr, "--sim=[rf|xp]                        Simulator interface: rf = RealFligt, xp = XPlane. Example: --sim=rf\n");
-    printf("--simport=[port]                     Port oft the simulator host.");
+    fprintf(stderr, "--simip=[ip]                         IP-Address oft the simulator host. If not specified localhost (127.0.0.1) is used.\n");
-    printf("--useimu                             Use IMU sensor data from the simulator instead of using attitude data from the simulator directly (experimental, not recommended).");
+    fprintf(stderr, "--simport=[port]                     Port oft the simulator host.\n");
-    printf("--chanmap=[mapstring]                Channel mapping, Maps INAVs motor and servo PWM outputs to the virtual receiver output in the simulator.\n");
+    fprintf(stderr, "--useimu                             Use IMU sensor data from the simulator instead of using attitude data from the simulator directly (experimental, not recommended).\n");
-    printf("                                     The mapstring has the following format: M(otor)|S(servo)<INAV-OUT>-<RECEIVER_OUT>,)... All numbers must have two digits\n");
+    fprintf(stderr, "--chanmap=[mapstring]                Channel mapping. Maps INAVs motor and servo PWM outputs to the virtual receiver output in the simulator.\n");
-    printf("                                     For example map motor 1 to virtal receiver output 1, servo 1 to output 2 and servo 2 to output 3:\n");
+    fprintf(stderr, "                                     The mapstring has the following format: M(otor)|S(servo)<INAV-OUT>-<RECEIVER-OUT>,... All numbers must have two digits\n");
-    printf("                                     --chanmap=M01-01,S01-02,S02-03\n"); 
+    fprintf(stderr, "                                     For example: Map motor 1 to virtal receiver output 1, servo 1 to output 2 and servo 2 to output 3:\n");
+    fprintf(stderr, "                                     --chanmap=M01-01,S01-02,S02-03\n"); 
 }
 
 void parseArguments(int argc, char *argv[])
 {
     int c;
-    while(1) {
+    while(true) {
         static struct option longOpt[] = {
             {"sim", optional_argument, 0, 's'},
             {"useimu", optional_argument, 0, 'u'},
@@ -171,6 +183,7 @@ void parseArguments(int argc, char *argv[])
             {"simip", optional_argument, 0, 'i'},
             {"simport", optional_argument, 0, 'p'},
             {"help", optional_argument, 0, 'h'},
+            {"path", optional_argument, 0, 'e'},
             {NULL, 0, NULL, 0}
         };
 
@@ -185,13 +198,13 @@ void parseArguments(int argc, char *argv[])
                 } else if (strcmp(optarg, "xp") == 0){
                     sitlSim = SITL_SIM_XPLANE;
                 } else {
-                    printf("[SIM] Unsupported simulator %s.\n", optarg);
+                    fprintf(stderr, "[SIM] Unsupported simulator %s.\n", optarg);
                 }
                 break;
 
             case 'c':
                 if (!parseMapping(optarg) && sitlSim != SITL_SIM_NONE) {
-                    printf("[SIM] Invalid channel mapping string.\n");
+                    fprintf(stderr, "[SIM] Invalid channel mapping string.\n");
                     printCmdLineOptions();
                     exit(0);
                 }
@@ -205,7 +218,11 @@ void parseArguments(int argc, char *argv[])
             case 'i':
                 simIp = optarg;
                 break;
-
+            case 'e':
+                if (!configFileSetPath(optarg)){
+                    fprintf(stderr, "[EEPROM] Invalid path, using eeprom file in program directory\n.");
+                }
+                break;
             case 'h':
                 printCmdLineOptions();
                 exit(0);
@@ -230,17 +247,6 @@ void unlockMainPID(void)
 }
 
 // Replacements for system functions
-void microsleep(uint32_t usec) {
-    struct timespec ts;
-    ts.tv_sec = 0;
-    ts.tv_nsec = usec*1000UL;
-    while (nanosleep(&ts, &ts) == -1 && errno == EINTR) ;
-}
-
-void delayMicroseconds_real(uint32_t us) {
-    microsleep(us);
-}
-
 timeUs_t micros(void) {
     struct timespec now;
     clock_gettime(CLOCK_MONOTONIC, &now);
@@ -259,31 +265,31 @@ uint32_t millis(void) {
 
 void delayMicroseconds(timeUs_t us)
 {
-    timeUs_t now = micros();
+    usleep(us);
-    while (micros() - now < us);
 }
 
 void delay(timeMs_t ms)
 {
-    while (ms--)
+    delayMicroseconds(ms * 1000UL);
-        delayMicroseconds(1000);
 }
 
 void systemReset(void) 
 {
-    printf("[SYSTEM] Reset\n");
+    fprintf(stderr, "[SYSTEM] Reset\n");
     exit(0);
 }
 
 void systemResetToBootloader(void)
 {
-    printf("[SYSTEM] Reset to bootloader\n");
+    fprintf(stderr, "[SYSTEM] Reset to bootloader\n");
     exit(0);
 }
 
 void failureMode(failureMode_e mode) {
-    printf("[SYSTEM] Failure mode %d\n", mode);
+    fprintf(stderr, "[SYSTEM] Failure mode %d\n", mode);
-    while (1);
+    while (true) {
+        delay(1000);
+    };
 }
 
 // Even more dummys and stubs

src/main/target/SITL/target.h

@@ -65,6 +65,8 @@
 #define USE_FAKE_BARO
 #define USE_FAKE_MAG
 #define USE_GPS_FAKE
+#define USE_RANGEFINDER_FAKE
+#define USE_RX_SIM
 
 #undef USE_DASHBOARD
 #undef USE_TELEMETRY_LTM