src/main/drivers/accgyro/accgyro_virtual.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_VIRTUAL_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_virtual.h"
  40
  41 static int16_t virtualGyroADC[XYZ_AXIS_COUNT];
  42 gyroDev_t *virtualGyroDev;
  43
  44 static void virtualGyroInit(gyroDev_t *gyro)
  45 {
  46     virtualGyroDev = 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 virtualGyroSet(gyroDev_t *gyro, int16_t x, int16_t y, int16_t z)
  55 {
  56     gyroDevLock(gyro);
  57
  58     virtualGyroADC[X] = x;
  59     virtualGyroADC[Y] = y;
  60     virtualGyroADC[Z] = z;
  61
  62     gyro->dataReady = true;
  63
  64     gyroDevUnLock(gyro);
  65 }
  66
  67 STATIC_UNIT_TESTED bool virtualGyroRead(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] = virtualGyroADC[X];
  77     gyro->gyroADCRaw[Y] = virtualGyroADC[Y];
  78     gyro->gyroADCRaw[Z] = virtualGyroADC[Z];
  79
  80     gyroDevUnLock(gyro);
  81     return true;
  82 }
  83
  84 static bool virtualGyroReadTemperature(gyroDev_t *gyro, int16_t *temperatureData)
  85 {
  86     UNUSED(gyro);
  87     UNUSED(temperatureData);
  88     return true;
  89 }
  90
  91 bool virtualGyroDetect(gyroDev_t *gyro)
  92 {
  93     gyro->initFn = virtualGyroInit;
  94     gyro->readFn = virtualGyroRead;
  95     gyro->temperatureFn = virtualGyroReadTemperature;
  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_VIRTUAL_GYRO
 104
 105 #ifdef USE_VIRTUAL_ACC
 106
 107 static int16_t virtualAccData[XYZ_AXIS_COUNT];
 108 accDev_t *virtualAccDev;
 109
 110 static void virtualAccInit(accDev_t *acc)
 111 {
 112     virtualAccDev = acc;
 113 #if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
 114     if (pthread_mutex_init(&acc->lock, NULL) != 0) {
 115         printf("Create acc lock error!\n");
 116     }
 117 #endif
 118 }
 119
 120 void virtualAccSet(accDev_t *acc, int16_t x, int16_t y, int16_t z)
 121 {
 122     accDevLock(acc);
 123
 124     virtualAccData[X] = x;
 125     virtualAccData[Y] = y;
 126     virtualAccData[Z] = z;
 127
 128     acc->dataReady = true;
 129
 130     accDevUnLock(acc);
 131 }
 132
 133 static bool virtualAccRead(accDev_t *acc)
 134 {
 135     accDevLock(acc);
 136     if (acc->dataReady == false) {
 137         accDevUnLock(acc);
 138         return false;
 139     }
 140     acc->dataReady = false;
 141
 142     acc->ADCRaw[X] = virtualAccData[X];
 143     acc->ADCRaw[Y] = virtualAccData[Y];
 144     acc->ADCRaw[Z] = virtualAccData[Z];
 145
 146     accDevUnLock(acc);
 147     return true;
 148 }
 149
 150 bool virtualAccDetect(accDev_t *acc)
 151 {
 152     acc->initFn = virtualAccInit;
 153     acc->readFn = virtualAccRead;
 154     acc->revisionCode = 0;
 155     return true;
 156 }
 157 #endif // USE_VIRTUAL_ACC