src/main/drivers/accgyro/accgyro_fake.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdint.h>
  20 #include <pthread.h>
  21
  22 #include "platform.h"
  23
  24 #include "common/axis.h"
  25 #include "common/utils.h"
  26
  27 #include "fc/runtime_config.h"
  28
  29 #include "drivers/accgyro/accgyro.h"
  30 #include "drivers/accgyro/accgyro_fake.h"
  31
  32
  33 #ifdef USE_IMU_FAKE
  34
  35 static float fakeGyroADC[XYZ_AXIS_COUNT];
  36
  37 static void fakeGyroInit(gyroDev_t *gyro)
  38 {
  39     UNUSED(gyro);
  40 }
  41
  42 void fakeGyroSet(int16_t x, int16_t y, int16_t z)
  43 {
  44     fakeGyroADC[X] = x;
  45     fakeGyroADC[Y] = y;
  46     fakeGyroADC[Z] = z;
  47 }
  48
  49 static bool fakeGyroRead(gyroDev_t *gyro)
  50 {
  51     gyro->gyroADCRaw[X] = fakeGyroADC[X];
  52     gyro->gyroADCRaw[Y] = fakeGyroADC[Y];
  53     gyro->gyroADCRaw[Z] = fakeGyroADC[Z];
  54     return true;
  55 }
  56
  57 static bool fakeGyroReadTemperature(gyroDev_t *gyro, int16_t *temperatureData)
  58 {
  59     UNUSED(gyro);
  60     UNUSED(temperatureData);
  61     return true;
  62 }
  63
  64 static bool fakeGyroInitStatus(gyroDev_t *gyro)
  65 {
  66     UNUSED(gyro);
  67     return true;
  68 }
  69
  70 bool fakeGyroDetect(gyroDev_t *gyro)
  71 {
  72     gyro->initFn = fakeGyroInit;
  73     gyro->intStatusFn = fakeGyroInitStatus;
  74     gyro->readFn = fakeGyroRead;
  75     gyro->temperatureFn = fakeGyroReadTemperature;
  76     gyro->scale = 0.0625f;
  77     gyro->gyroAlign = 0;
  78     return true;
  79 }
  80
  81 static int16_t fakeAccData[XYZ_AXIS_COUNT];
  82
  83 static void fakeAccInit(accDev_t *acc)
  84 {
  85     acc->acc_1G = 9806;
  86 }
  87
  88 void fakeAccSet(int16_t x, int16_t y, int16_t z)
  89 {
  90     fakeAccData[X] = x;
  91     fakeAccData[Y] = y;
  92     fakeAccData[Z] = z;
  93 }
  94
  95 static bool fakeAccRead(accDev_t *acc)
  96 {
  97     acc->ADCRaw[X] = fakeAccData[X];
  98     acc->ADCRaw[Y] = fakeAccData[Y];
  99     acc->ADCRaw[Z] = fakeAccData[Z];
 100     return true;
 101 }
 102
 103 bool fakeAccDetect(accDev_t *acc)
 104 {
 105     acc->initFn = fakeAccInit;
 106     acc->readFn = fakeAccRead;
 107     acc->accAlign = 0;
 108     return true;
 109 }
 110 #endif
 111