Merge pull request #2849 from martinbudden/bf_fake_accgyro_locks

Added locking to fake gyro and acc

src/main/common/printf.h

@@ -113,9 +113,6 @@ int tfp_sprintf(char *s, const char *fmt, ...);
 
 int tfp_format(void *putp, void (*putf) (void *, char), const char *fmt, va_list va);
 
-#define printf tfp_printf
-#define sprintf tfp_sprintf
-
 struct serialPort_s;
 void setPrintfSerialPort(struct serialPort_s *serialPort);
 void printfSupportInit(void);

src/main/drivers/accgyro.h

@@ -17,10 +17,14 @@
 
 #pragma once
 
+#include "platform.h"
 #include "common/axis.h"
 #include "drivers/exti.h"
 #include "drivers/sensor.h"
 #include "drivers/accgyro_mpu.h"
+#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
+#include <pthread.h>
+#endif
 
 #ifndef MPU_I2C_INSTANCE
 #define MPU_I2C_INSTANCE I2C_DEVICE
@@ -57,7 +61,10 @@ typedef struct gyroDev_s {
     uint8_t lpf;
     gyroRateKHz_e gyroRateKHz;
     uint8_t mpuDividerDrops;
-    volatile bool dataReady;
+    bool dataReady;
+#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
+    pthread_mutex_t lock;
+#endif
     sensor_align_e gyroAlign;
     mpuDetectionResult_t mpuDetectionResult;
     const extiConfig_t *mpuIntExtiConfig;
@@ -71,7 +78,47 @@ typedef struct accDev_s {
     uint16_t acc_1G;
     int16_t ADCRaw[XYZ_AXIS_COUNT];
     char revisionCode;                                      // a revision code for the sensor, if known
+    bool dataReady;
+#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
+    pthread_mutex_t lock;
+#endif
     sensor_align_e accAlign;
     mpuDetectionResult_t mpuDetectionResult;
     mpuConfiguration_t mpuConfiguration;
 } accDev_t;
+
+static inline void accDevLock(accDev_t *acc)
+{
+#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
+    pthread_mutex_lock(&acc->lock);
+#else
+    (void)acc;
+#endif
+}
+
+static inline void accDevUnLock(accDev_t *acc)
+{
+#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
+    pthread_mutex_unlock(&acc->lock);
+#else
+    (void)acc;
+#endif
+}
+
+static inline void gyroDevLock(gyroDev_t *gyro)
+{
+#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
+    pthread_mutex_lock(&gyro->lock);
+#else
+    (void)gyro;
+#endif
+}
+
+static inline void gyroDevUnLock(gyroDev_t *gyro)
+{
+#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
+    pthread_mutex_unlock(&gyro->lock);
+#else
+    (void)gyro;
+#endif
+}

src/main/drivers/accgyro_fake.c

@@ -23,23 +23,6 @@
 #if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
 #include <stdio.h>
 #include <pthread.h>
-#define printf printf
-#define sprintf sprintf
-
-#define ACC_LOCK pthread_mutex_unlock(&accUpdateLock)
-#define ACC_UNLOCK pthread_mutex_unlock(&accUpdateLock)
-
-#define GYRO_LOCK pthread_mutex_unlock(&gyroUpdateLock)
-#define GYRO_UNLOCK pthread_mutex_unlock(&gyroUpdateLock)
-
-#else
-
-#define ACC_LOCK
-#define ACC_UNLOCK
-
-#define GYRO_LOCK
-#define GYRO_UNLOCK
-
 #endif
 
 #ifdef USE_FAKE_GYRO
@@ -51,54 +34,45 @@
 #include "accgyro_fake.h"
 
 static int16_t fakeGyroADC[XYZ_AXIS_COUNT];
-
+gyroDev_t *fakeGyroDev;
-#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
-static pthread_mutex_t gyroUpdateLock;
-#endif
-#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_GYRO_SYNC)
-static bool gyroUpdated = false;
-#endif
 
 static void fakeGyroInit(gyroDev_t *gyro)
 {
-    UNUSED(gyro);
+    fakeGyroDev = gyro;
 #if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
-    if (pthread_mutex_init(&gyroUpdateLock, NULL) != 0) {
+    if (pthread_mutex_init(&gyro->lock, NULL) != 0) {
-        printf("Create gyroUpdateLock error!\n");
+        printf("Create gyro lock error!\n");
     }
 #endif
 }
 
-void fakeGyroSet(int16_t x, int16_t y, int16_t z)
+void fakeGyroSet(gyroDev_t *gyro, int16_t x, int16_t y, int16_t z)
 {
-    GYRO_LOCK;
+    gyroDevLock(gyro);
 
     fakeGyroADC[X] = x;
     fakeGyroADC[Y] = y;
     fakeGyroADC[Z] = z;
 
-#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_GYRO_SYNC)
+    gyro->dataReady = true;
-    gyroUpdated = true;
+
-#endif
+    gyroDevUnLock(gyro);
-    GYRO_UNLOCK;
 }
 
 static bool fakeGyroRead(gyroDev_t *gyro)
 {
-    GYRO_LOCK;
+    gyroDevLock(gyro);
-#if defined(SIMULATOR_GYRO_SYNC)
+    if (gyro->dataReady == false) {
-    if(gyroUpdated == false) {
+        gyroDevUnLock(gyro);
-        GYRO_UNLOCK;
         return false;
     }
-    gyroUpdated = false;
+    gyro->dataReady = false;
-#endif
 
     gyro->gyroADCRaw[X] = fakeGyroADC[X];
     gyro->gyroADCRaw[Y] = fakeGyroADC[Y];
     gyro->gyroADCRaw[Z] = fakeGyroADC[Z];
 
-    GYRO_UNLOCK;
+    gyroDevUnLock(gyro);
     return true;
 }
 
@@ -134,54 +108,45 @@ bool fakeGyroDetect(gyroDev_t *gyro)
 #ifdef USE_FAKE_ACC
 
 static int16_t fakeAccData[XYZ_AXIS_COUNT];
-
+accDev_t *fakeAccDev;
-#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
-static pthread_mutex_t accUpdateLock;
-#endif
-#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_ACC_SYNC)
-static bool accUpdated = false;
-#endif
 
 static void fakeAccInit(accDev_t *acc)
 {
-    UNUSED(acc);
+    fakeAccDev = acc;
 #if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
-    if (pthread_mutex_init(&accUpdateLock, NULL) != 0) {
+    if (pthread_mutex_init(&acc->lock, NULL) != 0) {
-        printf("Create accUpdateLock error!\n");
+        printf("Create acc lock error!\n");
     }
 #endif
 }
 
-void fakeAccSet(int16_t x, int16_t y, int16_t z)
+void fakeAccSet(accDev_t *acc, int16_t x, int16_t y, int16_t z)
 {
-    ACC_LOCK;
+    accDevLock(acc);
 
     fakeAccData[X] = x;
     fakeAccData[Y] = y;
     fakeAccData[Z] = z;
 
-#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_ACC_SYNC)
+    acc->dataReady = true;
-    accUpdated = true;
+
-#endif
+    accDevUnLock(acc);
-    ACC_LOCK;
 }
 
 static bool fakeAccRead(accDev_t *acc)
 {
-    ACC_LOCK;
+    accDevLock(acc);
-#if defined(SIMULATOR_ACC_SYNC)
+    if (acc->dataReady == false) {
-    if(accUpdated == false) {
+        accDevUnLock(acc);
-        ACC_UNLOCK;
         return false;
     }
-    accUpdated = false;
+    acc->dataReady = false;
-#endif
 
     acc->ADCRaw[X] = fakeAccData[X];
     acc->ADCRaw[Y] = fakeAccData[Y];
     acc->ADCRaw[Z] = fakeAccData[Z];
 
-    ACC_UNLOCK;
+    accDevUnLock(acc);
     return true;
 }
 

src/main/drivers/accgyro_fake.h

@@ -18,9 +18,11 @@
 #pragma once
 
 struct accDev_s;
+extern struct accDev_s *fakeAccDev;
 bool fakeAccDetect(struct accDev_s *acc);
-void fakeAccSet(int16_t x, int16_t y, int16_t z);
+void fakeAccSet(struct accDev_s *acc, int16_t x, int16_t y, int16_t z);
 
 struct gyroDev_s;
+extern struct gyroDev_s *fakeGyroDev;
 bool fakeGyroDetect(struct gyroDev_s *gyro);
-void fakeGyroSet(int16_t x, int16_t y, int16_t z);
+void fakeGyroSet(struct gyroDev_s *gyro, int16_t x, int16_t y, int16_t z);

src/main/drivers/io.c

@@ -15,14 +15,14 @@
  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#include "platform.h"
+
 #include "io.h"
 #include "io_impl.h"
 #include "rcc.h"
 
 #include "common/utils.h"
 
-#include "target.h"
-
 // io ports defs are stored in array by index now
 struct ioPortDef_s {
     rccPeriphTag_t rcc;

src/main/drivers/io.h

@@ -21,6 +21,7 @@
 #include <stdint.h>
 
 #include <platform.h>
+
 #include "resource.h"
 
 #include "io_types.h"

src/main/drivers/io_impl.h

@@ -18,9 +18,10 @@
  #pragma once
 
 // TODO - GPIO_TypeDef include
-#include "io.h"
 #include "platform.h"
 
+#include "io.h"
+
 typedef struct ioDef_s {
     ioTag_t tag;
 } ioDef_t;

src/main/fc/cli.c

@@ -1883,12 +1883,12 @@ static void cliSerialPassthrough(char *cmdline)
         tok = strtok_r(NULL, " ", &saveptr);
     }
 
-    printf("Port %d ", id);
+    tfp_printf("Port %d ", id);
     serialPort_t *passThroughPort;
     serialPortUsage_t *passThroughPortUsage = findSerialPortUsageByIdentifier(id);
     if (!passThroughPortUsage || passThroughPortUsage->serialPort == NULL) {
         if (!baud) {
-            printf("closed, specify baud.\r\n");
+            tfp_printf("closed, specify baud.\r\n");
             return;
         }
         if (!mode)
@@ -1898,17 +1898,17 @@ static void cliSerialPassthrough(char *cmdline)
                                          baud, mode,
                                          SERIAL_NOT_INVERTED);
         if (!passThroughPort) {
-            printf("could not be opened.\r\n");
+            tfp_printf("could not be opened.\r\n");
             return;
         }
-        printf("opened, baud = %d.\r\n", baud);
+        tfp_printf("opened, baud = %d.\r\n", baud);
     } else {
         passThroughPort = passThroughPortUsage->serialPort;
         // If the user supplied a mode, override the port's mode, otherwise
         // leave the mode unchanged. serialPassthrough() handles one-way ports.
-        printf("already open.\r\n");
+        tfp_printf("already open.\r\n");
         if (mode && passThroughPort->mode != mode) {
-            printf("mode changed from %d to %d.\r\n",
+            tfp_printf("mode changed from %d to %d.\r\n",
                    passThroughPort->mode, mode);
             serialSetMode(passThroughPort, mode);
         }
@@ -1919,7 +1919,7 @@ static void cliSerialPassthrough(char *cmdline)
         }
     }
 
-    printf("forwarding, power cycle to exit.\r\n");
+    tfp_printf("forwarding, power cycle to exit.\r\n");
 
     serialPassthrough(cliPort, passThroughPort, NULL, NULL);
 }
@@ -3202,11 +3202,11 @@ static void cliGpsPassthrough(char *cmdline)
 static int parseEscNumber(char *pch, bool allowAllEscs) {
     int escNumber = atoi(pch);
     if ((escNumber >= 0) && (escNumber < getMotorCount())) {
-        printf("Programming on ESC %d.\r\n", escNumber);
+        tfp_printf("Programming on ESC %d.\r\n", escNumber);
     } else if (allowAllEscs && escNumber == ALL_ESCS) {
-        printf("Programming on all ESCs.\r\n");
+        tfp_printf("Programming on all ESCs.\r\n");
     } else {
-        printf("Invalid ESC number, range: 0 to %d.\r\n", getMotorCount() - 1);
+        tfp_printf("Invalid ESC number, range: 0 to %d.\r\n", getMotorCount() - 1);
 
         return -1;
     }
@@ -3254,9 +3254,9 @@ static void cliDshotProg(char *cmdline)
                         delay(10); // wait for sound output to finish
                     }
 
-                    printf("Command %d written.\r\n", command);
+                    tfp_printf("Command %d written.\r\n", command);
                 } else {
-                    printf("Invalid command, range 1 to %d.\r\n", DSHOT_MIN_THROTTLE - 1);
+                    tfp_printf("Invalid command, range 1 to %d.\r\n", DSHOT_MIN_THROTTLE - 1);
                 }
 
                 break;
@@ -3691,7 +3691,7 @@ static void cliTasks(char *cmdline)
         getTaskInfo(taskId, &taskInfo);
         if (taskInfo.isEnabled) {
             int taskFrequency;
-            int subTaskFrequency;
+            int subTaskFrequency = 0;
             if (taskId == TASK_GYROPID) {
                 subTaskFrequency = taskInfo.latestDeltaTime == 0 ? 0 : (int)(1000000.0f / ((float)taskInfo.latestDeltaTime));
                 taskFrequency = subTaskFrequency / pidConfig()->pid_process_denom;

src/main/fc/fc_core.c

@@ -477,7 +477,7 @@ void processRx(timeUs_t currentTimeUs)
 
 static void subTaskPidController(void)
 {
-    uint32_t startTime;
+    uint32_t startTime = 0;
     if (debugMode == DEBUG_PIDLOOP) {startTime = micros();}
     // PID - note this is function pointer set by setPIDController()
     pidController(currentPidProfile, &accelerometerConfig()->accelerometerTrims);
@@ -486,7 +486,7 @@ static void subTaskPidController(void)
 
 static void subTaskMainSubprocesses(timeUs_t currentTimeUs)
 {
-    uint32_t startTime;
+    uint32_t startTime = 0;
     if (debugMode == DEBUG_PIDLOOP) {startTime = micros();}
 
     // Read out gyro temperature if used for telemmetry
@@ -560,7 +560,7 @@ static void subTaskMainSubprocesses(timeUs_t currentTimeUs)
 
 static void subTaskMotorUpdate(void)
 {
-    uint32_t startTime;
+    uint32_t startTime = 0;
     if (debugMode == DEBUG_CYCLETIME) {
         startTime = micros();
         static uint32_t previousMotorUpdateTime;
@@ -621,7 +621,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
     // 1 - pidController()
     // 2 - subTaskMainSubprocesses()
     // 3 - subTaskMotorUpdate()
-    uint32_t startTime;
+    uint32_t startTime = 0;
     if (debugMode == DEBUG_PIDLOOP) {startTime = micros();}
     gyroUpdate();
     DEBUG_SET(DEBUG_PIDLOOP, 0, micros() - startTime);

src/main/flight/imu.c

@@ -52,8 +52,6 @@
 #if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
 #include <stdio.h>
 #include <pthread.h>
-#define printf printf
-#define sprintf sprintf
 
 static pthread_mutex_t imuUpdateLock;
 

src/main/io/ledstrip.c

@@ -377,7 +377,7 @@ void generateLedConfig(ledConfig_t *ledConfig, char *ledConfigBuffer, size_t buf
     *fptr = 0;
 
     // TODO - check buffer length
-    sprintf(ledConfigBuffer, "%u,%u:%s:%s:%u", ledGetX(ledConfig), ledGetY(ledConfig), directions, baseFunctionOverlays, ledGetColor(ledConfig));
+    tfp_sprintf(ledConfigBuffer, "%u,%u:%s:%s:%u", ledGetX(ledConfig), ledGetY(ledConfig), directions, baseFunctionOverlays, ledGetColor(ledConfig));
 }
 
 typedef enum {

src/main/io/osd.c

@@ -74,6 +74,7 @@
 #include "fc/rc_controls.h"
 #include "fc/runtime_config.h"
 
+#include "flight/altitude.h"
 #include "flight/imu.h"
 #include "flight/altitude.h"
 
@@ -189,14 +190,14 @@ static void osdDrawSingleElement(uint8_t item)
                 osdRssi = 99;
 
             buff[0] = SYM_RSSI;
-            sprintf(buff + 1, "%d", osdRssi);
+            tfp_sprintf(buff + 1, "%d", osdRssi);
             break;
         }
 
         case OSD_MAIN_BATT_VOLTAGE:
         {
             buff[0] = SYM_BATT_5;
-            sprintf(buff + 1, "%d.%1dV", getBatteryVoltage() / 10, getBatteryVoltage() % 10);
+            tfp_sprintf(buff + 1, "%d.%1dV", getBatteryVoltage() / 10, getBatteryVoltage() % 10);
             break;
         }
 
@@ -204,14 +205,14 @@ static void osdDrawSingleElement(uint8_t item)
         {
             int32_t amperage = getAmperage();
             buff[0] = SYM_AMP;
-            sprintf(buff + 1, "%d.%02d", abs(amperage) / 100, abs(amperage) % 100);
+            tfp_sprintf(buff + 1, "%d.%02d", abs(amperage) / 100, abs(amperage) % 100);
             break;
         }
 
         case OSD_MAH_DRAWN:
         {
             buff[0] = SYM_MAH;
-            sprintf(buff + 1, "%d", getMAhDrawn());
+            tfp_sprintf(buff + 1, "%d", getMAhDrawn());
             break;
         }
 
@@ -219,14 +220,14 @@ static void osdDrawSingleElement(uint8_t item)
         case OSD_GPS_SATS:
         {
             buff[0] = 0x1f;
-            sprintf(buff + 1, "%d", GPS_numSat);
+            tfp_sprintf(buff + 1, "%d", GPS_numSat);
             break;
         }
 
         case OSD_GPS_SPEED:
         {
             // FIXME ideally we want to use SYM_KMH symbol but it's not in the font any more, so we use K.
-            sprintf(buff, "%dK", CM_S_TO_KM_H(GPS_speed) * 10);
+            tfp_sprintf(buff, "%dK", CM_S_TO_KM_H(GPS_speed) * 10);
             break;
         }
 
@@ -258,7 +259,7 @@ static void osdDrawSingleElement(uint8_t item)
         case OSD_ALTITUDE:
         {
             int32_t alt = osdGetAltitude(getEstimatedAltitude());
-            sprintf(buff, "%c%d.%01d%c", alt < 0 ? '-' : ' ', abs(alt / 100), abs((alt % 100) / 10), osdGetAltitudeSymbol());
+            tfp_sprintf(buff, "%c%d.%01d%c", alt < 0 ? '-' : ' ', abs(alt / 100), abs((alt % 100) / 10), osdGetAltitudeSymbol());
             break;
         }
 
@@ -266,14 +267,14 @@ static void osdDrawSingleElement(uint8_t item)
         {
             uint32_t seconds = micros() / 1000000;
             buff[0] = SYM_ON_M;
-            sprintf(buff + 1, "%02d:%02d", seconds / 60, seconds % 60);
+            tfp_sprintf(buff + 1, "%02d:%02d", seconds / 60, seconds % 60);
             break;
         }
 
         case OSD_FLYTIME:
         {
             buff[0] = SYM_FLY_M;
-            sprintf(buff + 1, "%02d:%02d", flyTime / 60, flyTime % 60);
+            tfp_sprintf(buff + 1, "%02d:%02d", flyTime / 60, flyTime % 60);
             break;
         }
 
@@ -314,7 +315,7 @@ static void osdDrawSingleElement(uint8_t item)
         {
             buff[0] = SYM_THR;
             buff[1] = SYM_THR1;
-            sprintf(buff + 2, "%d", (constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN));
+            tfp_sprintf(buff + 2, "%d", (constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN));
             break;
         }
 
@@ -325,9 +326,9 @@ static void osdDrawSingleElement(uint8_t item)
 #if defined(VTX)
             const char vtxBandLetter = vtx58BandLetter[vtxConfig()->vtx_band + 1];
             const char *vtxChannelName = vtx58ChannelNames[vtxConfig()->vtx_channel + 1];
-            sprintf(buff, "%c:%s", vtxBandLetter, vtxChannelName);
+            tfp_sprintf(buff, "%c:%s", vtxBandLetter, vtxChannelName);
 #elif defined(USE_RTC6705)
-            sprintf(buff, "CH:%d", current_vtx_channel % CHANNELS_PER_BAND + 1);
+            tfp_sprintf(buff, "CH:%d", current_vtx_channel % CHANNELS_PER_BAND + 1);
 #endif
             break;
         }
@@ -410,27 +411,27 @@ static void osdDrawSingleElement(uint8_t item)
         case OSD_ROLL_PIDS:
         {
             const pidProfile_t *pidProfile = currentPidProfile;
-            sprintf(buff, "ROL %3d %3d %3d", pidProfile->P8[PIDROLL], pidProfile->I8[PIDROLL], pidProfile->D8[PIDROLL]);
+            tfp_sprintf(buff, "ROL %3d %3d %3d", pidProfile->P8[PIDROLL], pidProfile->I8[PIDROLL], pidProfile->D8[PIDROLL]);
             break;
         }
 
         case OSD_PITCH_PIDS:
         {
             const pidProfile_t *pidProfile = currentPidProfile;
-            sprintf(buff, "PIT %3d %3d %3d", pidProfile->P8[PIDPITCH], pidProfile->I8[PIDPITCH], pidProfile->D8[PIDPITCH]);
+            tfp_sprintf(buff, "PIT %3d %3d %3d", pidProfile->P8[PIDPITCH], pidProfile->I8[PIDPITCH], pidProfile->D8[PIDPITCH]);
             break;
         }
 
         case OSD_YAW_PIDS:
         {
             const pidProfile_t *pidProfile = currentPidProfile;
-            sprintf(buff, "YAW %3d %3d %3d", pidProfile->P8[PIDYAW], pidProfile->I8[PIDYAW], pidProfile->D8[PIDYAW]);
+            tfp_sprintf(buff, "YAW %3d %3d %3d", pidProfile->P8[PIDYAW], pidProfile->I8[PIDYAW], pidProfile->D8[PIDYAW]);
             break;
         }
 
         case OSD_POWER:
         {
-            sprintf(buff, "%dW", getAmperage() * getBatteryVoltage() / 1000);
+            tfp_sprintf(buff, "%dW", getAmperage() * getBatteryVoltage() / 1000);
             break;
         }
 
@@ -438,7 +439,7 @@ static void osdDrawSingleElement(uint8_t item)
         {
             const uint8_t pidProfileIndex = getCurrentPidProfileIndex();
             const uint8_t rateProfileIndex = getCurrentControlRateProfileIndex();
-            sprintf(buff, "%d-%d", pidProfileIndex + 1, rateProfileIndex + 1);
+            tfp_sprintf(buff, "%d-%d", pidProfileIndex + 1, rateProfileIndex + 1);
             break;
         }
 
@@ -446,11 +447,11 @@ static void osdDrawSingleElement(uint8_t item)
         {
             switch(getBatteryState()) {
                 case BATTERY_WARNING:
-                    sprintf(buff, "LOW BATTERY");
+                    tfp_sprintf(buff, "LOW BATTERY");
                     break;
 
                 case BATTERY_CRITICAL:
-                    sprintf(buff, "LAND NOW");
+                    tfp_sprintf(buff, "LAND NOW");
                     elemOffsetX += 1;
                     break;
 
@@ -464,7 +465,7 @@ static void osdDrawSingleElement(uint8_t item)
         {
             uint16_t cellV = getBatteryVoltage() * 10 / getBatteryCellCount();
             buff[0] = SYM_BATT_5;
-            sprintf(buff + 1, "%d.%dV", cellV / 100, cellV % 100);
+            tfp_sprintf(buff + 1, "%d.%dV", cellV / 100, cellV % 100);
             break;
         }
 
@@ -603,7 +604,7 @@ void osdInit(displayPort_t *osdDisplayPortToUse)
     osdDrawLogo(3, 1);
 
     char string_buffer[30];
-    sprintf(string_buffer, "V%s", FC_VERSION_STRING);
+    tfp_sprintf(string_buffer, "V%s", FC_VERSION_STRING);
     displayWrite(osdDisplayPort, 20, 6, string_buffer);
 #ifdef CMS
     displayWrite(osdDisplayPort, 7, 8,  CMS_STARTUP_HELP_TEXT1);
@@ -763,7 +764,7 @@ static void osdShowStats(void)
     }
 
     displayWrite(osdDisplayPort, 2, top, "MIN BATTERY      :");
-    sprintf(buff, "%d.%1dV", stats.min_voltage / 10, stats.min_voltage % 10);
+    tfp_sprintf(buff, "%d.%1dV", stats.min_voltage / 10, stats.min_voltage % 10);
     displayWrite(osdDisplayPort, 22, top++, buff);
 
     displayWrite(osdDisplayPort, 2, top, "MIN RSSI         :");
@@ -785,7 +786,7 @@ static void osdShowStats(void)
 
     displayWrite(osdDisplayPort, 2, top, "MAX ALTITUDE     :");
     int32_t alt = osdGetAltitude(stats.max_altitude);
-    sprintf(buff, "%c%d.%01d%c", alt < 0 ? '-' : ' ', abs(alt / 100), abs((alt % 100) / 10), osdGetAltitudeSymbol());
+    tfp_sprintf(buff, "%c%d.%01d%c", alt < 0 ? '-' : ' ', abs(alt / 100), abs((alt % 100) / 10), osdGetAltitudeSymbol());
     displayWrite(osdDisplayPort, 22, top++, buff);
 
 #ifdef BLACKBOX

src/main/io/osd_slave.c

@@ -98,7 +98,7 @@ void osdSlaveInit(displayPort_t *osdDisplayPortToUse)
     osdDrawLogo(3, 1);
 
     char string_buffer[30];
-    sprintf(string_buffer, "V%s", FC_VERSION_STRING);
+    tfp_sprintf(string_buffer, "V%s", FC_VERSION_STRING);
     displayWrite(osdDisplayPort, 20, 6, string_buffer);
     displayWrite(osdDisplayPort, 13, 6, "OSD");
 
@@ -139,7 +139,7 @@ void osdSlaveUpdate(timeUs_t currentTimeUs)
 #ifdef OSD_SLAVE_DEBUG
     char buff[32];
     for (int i = 0; i < 4; i ++) {
-        sprintf(buff, "%5d", debug[i]);
+        tfp_sprintf(buff, "%5d", debug[i]);
         displayWrite(osdDisplayPort, i * 8, 0, buff);
     }
 #endif

src/main/target/SITL/target.c

@@ -19,8 +19,6 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <math.h>
-#define printf printf
-#define sprintf sprintf
 
 #include <errno.h>
 #include <time.h>
@@ -96,13 +94,13 @@ void updateState(const fdm_packet* pkt) {
 	x = -pkt->imu_linear_acceleration_xyz[0] * ACC_SCALE;
 	y = -pkt->imu_linear_acceleration_xyz[1] * ACC_SCALE;
 	z = -pkt->imu_linear_acceleration_xyz[2] * ACC_SCALE;
-	fakeAccSet(x, y, z);
+	fakeAccSet(fakeAccDev, x, y, z);
 //	printf("[acc]%lf,%lf,%lf\n", pkt->imu_linear_acceleration_xyz[0], pkt->imu_linear_acceleration_xyz[1], pkt->imu_linear_acceleration_xyz[2]);
 
 	x = pkt->imu_angular_velocity_rpy[0] * GYRO_SCALE * RAD2DEG;
 	y = -pkt->imu_angular_velocity_rpy[1] * GYRO_SCALE * RAD2DEG;
 	z = -pkt->imu_angular_velocity_rpy[2] * GYRO_SCALE * RAD2DEG;
-	fakeGyroSet(x, y, z);
+	fakeGyroSet(fakeGyroDev, x, y, z);
 //	printf("[gyr]%lf,%lf,%lf\n", pkt->imu_angular_velocity_rpy[0], pkt->imu_angular_velocity_rpy[1], pkt->imu_angular_velocity_rpy[2]);
 
 #if defined(SKIP_IMU_CALC)

src/main/target/SITL/target.h

@@ -162,7 +162,7 @@ typedef struct
   uint32_t BRR;
 } GPIO_TypeDef;
 
-#define GPIOA_BASE (0x0000)
+#define GPIOA_BASE (0x0001)
 
 typedef struct
 {