Added locking to fake gyro and acc

src/main/drivers/accgyro.h

@@ -57,7 +57,7 @@ typedef struct gyroDev_s {
     uint8_t lpf;
     gyroRateKHz_e gyroRateKHz;
     uint8_t mpuDividerDrops;
-    volatile bool dataReady;
+    bool dataReady;
     sensor_align_e gyroAlign;
     mpuDetectionResult_t mpuDetectionResult;
     const extiConfig_t *mpuIntExtiConfig;
@@ -71,6 +71,7 @@ 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;
     sensor_align_e accAlign;
     mpuDetectionResult_t mpuDetectionResult;
     mpuConfiguration_t mpuConfiguration;

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,65 @@
 #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;
+static pthread_mutex_t gyroLock;
-#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(&gyroLock, NULL) != 0) {
-        printf("Create gyroUpdateLock error!\n");
+        printf("Create gyroLock error!\n");
     }
 #endif
 }
 
-void fakeGyroSet(int16_t x, int16_t y, int16_t z)
+static void fakeGyroLock(gyroDev_t *gyro)
 {
-    GYRO_LOCK;
+    UNUSED(gyro);
+#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
+    pthread_mutex_lock(&gyroLock);
+#endif
+}
+
+static void fakeGyroUnLock(gyroDev_t *gyro)
+{
+    UNUSED(gyro);
+#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
+    pthread_mutex_unlock(&gyroLock);
+#endif
+}
+
+void fakeGyroSet(gyroDev_t *gyro, int16_t x, int16_t y, int16_t z)
+{
+    fakeGyroLock(gyro);
 
     fakeGyroADC[X] = x;
     fakeGyroADC[Y] = y;
     fakeGyroADC[Z] = z;
 
-#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_GYRO_SYNC)
+    gyro->dataReady = true;
-    gyroUpdated = true;
+
-#endif
+    fakeGyroUnLock(gyro);
-    GYRO_UNLOCK;
 }
 
 static bool fakeGyroRead(gyroDev_t *gyro)
 {
-    GYRO_LOCK;
+    fakeGyroLock(gyro);
-#if defined(SIMULATOR_GYRO_SYNC)
+    if (gyro->dataReady == false) {
-    if(gyroUpdated == false) {
+        fakeGyroUnLock(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;
+    fakeGyroUnLock(gyro);
     return true;
 }
 
@@ -134,54 +128,65 @@ 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;
+static pthread_mutex_t accLock;
-#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(&accLock, NULL) != 0) {
-        printf("Create accUpdateLock error!\n");
+        printf("Create accLock error!\n");
     }
 #endif
 }
 
-void fakeAccSet(int16_t x, int16_t y, int16_t z)
+static void fakeAccLock(accDev_t *acc)
 {
-    ACC_LOCK;
+    UNUSED(acc);
+#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
+    pthread_mutex_lock(&accLock);
+#endif
+}
+
+static void fakeAccUnLock(accDev_t *acc)
+{
+    UNUSED(acc);
+#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
+    pthread_mutex_unlock(&accLock);
+#endif
+}
+
+void fakeAccSet(accDev_t *acc, int16_t x, int16_t y, int16_t z)
+{
+    fakeAccLock(acc);
 
     fakeAccData[X] = x;
     fakeAccData[Y] = y;
     fakeAccData[Z] = z;
 
-#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_ACC_SYNC)
+    acc->dataReady = true;
-    accUpdated = true;
+
-#endif
+    fakeAccUnLock(acc);
-    ACC_LOCK;
 }
 
 static bool fakeAccRead(accDev_t *acc)
 {
-    ACC_LOCK;
+    fakeAccLock(acc);
-#if defined(SIMULATOR_ACC_SYNC)
+    if (acc->dataReady == false) {
-    if(accUpdated == false) {
+        fakeAccUnLock(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;
+    fakeAccUnLock(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/target/SITL/target.c

@@ -96,13 +96,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
 {