src/main/drivers/accgyro/accgyro_fake.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include <stdbool.h>
  22 #include <stdint.h>
  23
  24 #include "platform.h"
  25
  26 #if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
  27 #include <stdio.h>
  28 #include <pthread.h>
  29 #endif
  30
  31 #ifdef USE_FAKE_GYRO
  32
  33 #include "build/build_config.h"
  34
  35 #include "common/axis.h"
  36 #include "common/utils.h"
  37
  38 #include "drivers/accgyro/accgyro.h"
  39 #include "drivers/accgyro/accgyro_fake.h"
  40
  41 static int16_t fakeGyroADC[XYZ_AXIS_COUNT];
  42 gyroDev_t *fakeGyroDev;
  43
  44 static void fakeGyroInit(gyroDev_t *gyro)
  45 {
  46     fakeGyroDev = gyro;
  47 #if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
  48     if (pthread_mutex_init(&gyro->lock, NULL) != 0) {
  49         printf("Create gyro lock error!\n");
  50     }
  51 #endif
  52 }
  53
  54 void fakeGyroSet(gyroDev_t *gyro, int16_t x, int16_t y, int16_t z)
  55 {
  56     gyroDevLock(gyro);
  57
  58     fakeGyroADC[X] = x;
  59     fakeGyroADC[Y] = y;
  60     fakeGyroADC[Z] = z;
  61
  62     gyro->dataReady = true;
  63
  64     gyroDevUnLock(gyro);
  65 }
  66
  67 STATIC_UNIT_TESTED bool fakeGyroRead(gyroDev_t *gyro)
  68 {
  69     gyroDevLock(gyro);
  70     if (gyro->dataReady == false) {
  71         gyroDevUnLock(gyro);
  72         return false;
  73     }
  74     gyro->dataReady = false;
  75
  76     gyro->gyroADCRaw[X] = fakeGyroADC[X];
  77     gyro->gyroADCRaw[Y] = fakeGyroADC[Y];
  78     gyro->gyroADCRaw[Z] = fakeGyroADC[Z];
  79
  80     gyroDevUnLock(gyro);
  81     return true;
  82 }
  83
  84 static bool fakeGyroReadTemperature(gyroDev_t *gyro, int16_t *temperatureData)
  85 {
  86     UNUSED(gyro);
  87     UNUSED(temperatureData);
  88     return true;
  89 }
  90
  91 bool fakeGyroDetect(gyroDev_t *gyro)
  92 {
  93     gyro->initFn = fakeGyroInit;
  94     gyro->readFn = fakeGyroRead;
  95     gyro->temperatureFn = fakeGyroReadTemperature;
  96 #if defined(SIMULATOR_BUILD)
  97     gyro->scale = GYRO_SCALE_2000DPS;
  98 #else
  99     gyro->scale = 1.0f;
 100 #endif
 101     return true;
 102 }
 103 #endif // USE_FAKE_GYRO
 104
 105
 106 #ifdef USE_FAKE_ACC
 107
 108 static int16_t fakeAccData[XYZ_AXIS_COUNT];
 109 accDev_t *fakeAccDev;
 110
 111 static void fakeAccInit(accDev_t *acc)
 112 {
 113     fakeAccDev = acc;
 114 #if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
 115     if (pthread_mutex_init(&acc->lock, NULL) != 0) {
 116         printf("Create acc lock error!\n");
 117     }
 118 #endif
 119 }
 120
 121 void fakeAccSet(accDev_t *acc, int16_t x, int16_t y, int16_t z)
 122 {
 123     accDevLock(acc);
 124
 125     fakeAccData[X] = x;
 126     fakeAccData[Y] = y;
 127     fakeAccData[Z] = z;
 128
 129     acc->dataReady = true;
 130
 131     accDevUnLock(acc);
 132 }
 133
 134 static bool fakeAccRead(accDev_t *acc)
 135 {
 136     accDevLock(acc);
 137     if (acc->dataReady == false) {
 138         accDevUnLock(acc);
 139         return false;
 140     }
 141     acc->dataReady = false;
 142
 143     acc->ADCRaw[X] = fakeAccData[X];
 144     acc->ADCRaw[Y] = fakeAccData[Y];
 145     acc->ADCRaw[Z] = fakeAccData[Z];
 146
 147     accDevUnLock(acc);
 148     return true;
 149 }
 150
 151 bool fakeAccDetect(accDev_t *acc)
 152 {
 153     acc->initFn = fakeAccInit;
 154     acc->readFn = fakeAccRead;
 155     acc->revisionCode = 0;
 156     return true;
 157 }
 158 #endif // USE_FAKE_ACC