mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-13 19:40:31 +03:00
Rename FAKE to VIRTUAL (#12493)
* use_fake_xyz to use_virtual_xyz * xyz_fake to xyz_virtual * other fake to virtual * all files fake to vrtual * last touch on fake to virtual
This commit is contained in:
parent
4f5b99d34c
commit
515e55ef0a
29 changed files with 172 additions and 172 deletions
|
@ -137,20 +137,20 @@
|
||||||
const char * const lookupTableAccHardware[] = {
|
const char * const lookupTableAccHardware[] = {
|
||||||
"AUTO", "NONE", "ADXL345", "MPU6050", "MMA8452", "BMA280", "LSM303DLHC",
|
"AUTO", "NONE", "ADXL345", "MPU6050", "MMA8452", "BMA280", "LSM303DLHC",
|
||||||
"MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608G", "ICM20649", "ICM20689", "ICM42605", "ICM42688P",
|
"MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608G", "ICM20649", "ICM20689", "ICM42605", "ICM42688P",
|
||||||
"BMI160", "BMI270", "LSM6DSO", "FAKE"
|
"BMI160", "BMI270", "LSM6DSO", "VIRTUAL"
|
||||||
};
|
};
|
||||||
|
|
||||||
// sync with gyroHardware_e
|
// sync with gyroHardware_e
|
||||||
const char * const lookupTableGyroHardware[] = {
|
const char * const lookupTableGyroHardware[] = {
|
||||||
"AUTO", "NONE", "MPU6050", "L3G4200D", "MPU3050", "L3GD20",
|
"AUTO", "NONE", "MPU6050", "L3G4200D", "MPU3050", "L3GD20",
|
||||||
"MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608G", "ICM20649", "ICM20689", "ICM42605", "ICM42688P",
|
"MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608G", "ICM20649", "ICM20689", "ICM42605", "ICM42688P",
|
||||||
"BMI160", "BMI270", "LSM6SDO", "FAKE"
|
"BMI160", "BMI270", "LSM6SDO", "VIRTUAL"
|
||||||
};
|
};
|
||||||
|
|
||||||
#if defined(USE_SENSOR_NAMES) || defined(USE_BARO)
|
#if defined(USE_SENSOR_NAMES) || defined(USE_BARO)
|
||||||
// sync with baroSensor_e
|
// sync with baroSensor_e
|
||||||
const char * const lookupTableBaroHardware[] = {
|
const char * const lookupTableBaroHardware[] = {
|
||||||
"AUTO", "NONE", "BMP085", "MS5611", "BMP280", "LPS", "QMP6988", "BMP388", "DPS310", "2SMPB_02B", "FAKE"
|
"AUTO", "NONE", "BMP085", "MS5611", "BMP280", "LPS", "QMP6988", "BMP388", "DPS310", "2SMPB_02B", "VIRTUAL"
|
||||||
};
|
};
|
||||||
#endif
|
#endif
|
||||||
#if defined(USE_SENSOR_NAMES) || defined(USE_MAG)
|
#if defined(USE_SENSOR_NAMES) || defined(USE_MAG)
|
||||||
|
|
|
@ -59,7 +59,7 @@ typedef enum {
|
||||||
GYRO_BMI160,
|
GYRO_BMI160,
|
||||||
GYRO_BMI270,
|
GYRO_BMI270,
|
||||||
GYRO_LSM6DSO,
|
GYRO_LSM6DSO,
|
||||||
GYRO_FAKE
|
GYRO_VIRTUAL
|
||||||
} gyroHardware_e;
|
} gyroHardware_e;
|
||||||
|
|
||||||
typedef enum {
|
typedef enum {
|
||||||
|
|
|
@ -28,7 +28,7 @@
|
||||||
#include <pthread.h>
|
#include <pthread.h>
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef USE_FAKE_GYRO
|
#ifdef USE_VIRTUAL_GYRO
|
||||||
|
|
||||||
#include "build/build_config.h"
|
#include "build/build_config.h"
|
||||||
|
|
||||||
|
@ -36,14 +36,14 @@
|
||||||
#include "common/utils.h"
|
#include "common/utils.h"
|
||||||
|
|
||||||
#include "drivers/accgyro/accgyro.h"
|
#include "drivers/accgyro/accgyro.h"
|
||||||
#include "drivers/accgyro/accgyro_fake.h"
|
#include "drivers/accgyro/accgyro_virtual.h"
|
||||||
|
|
||||||
static int16_t fakeGyroADC[XYZ_AXIS_COUNT];
|
static int16_t virtualGyroADC[XYZ_AXIS_COUNT];
|
||||||
gyroDev_t *fakeGyroDev;
|
gyroDev_t *virtualGyroDev;
|
||||||
|
|
||||||
static void fakeGyroInit(gyroDev_t *gyro)
|
static void virtualGyroInit(gyroDev_t *gyro)
|
||||||
{
|
{
|
||||||
fakeGyroDev = gyro;
|
virtualGyroDev = gyro;
|
||||||
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
|
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
|
||||||
if (pthread_mutex_init(&gyro->lock, NULL) != 0) {
|
if (pthread_mutex_init(&gyro->lock, NULL) != 0) {
|
||||||
printf("Create gyro lock error!\n");
|
printf("Create gyro lock error!\n");
|
||||||
|
@ -51,20 +51,20 @@ static void fakeGyroInit(gyroDev_t *gyro)
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
void fakeGyroSet(gyroDev_t *gyro, int16_t x, int16_t y, int16_t z)
|
void virtualGyroSet(gyroDev_t *gyro, int16_t x, int16_t y, int16_t z)
|
||||||
{
|
{
|
||||||
gyroDevLock(gyro);
|
gyroDevLock(gyro);
|
||||||
|
|
||||||
fakeGyroADC[X] = x;
|
virtualGyroADC[X] = x;
|
||||||
fakeGyroADC[Y] = y;
|
virtualGyroADC[Y] = y;
|
||||||
fakeGyroADC[Z] = z;
|
virtualGyroADC[Z] = z;
|
||||||
|
|
||||||
gyro->dataReady = true;
|
gyro->dataReady = true;
|
||||||
|
|
||||||
gyroDevUnLock(gyro);
|
gyroDevUnLock(gyro);
|
||||||
}
|
}
|
||||||
|
|
||||||
STATIC_UNIT_TESTED bool fakeGyroRead(gyroDev_t *gyro)
|
STATIC_UNIT_TESTED bool virtualGyroRead(gyroDev_t *gyro)
|
||||||
{
|
{
|
||||||
gyroDevLock(gyro);
|
gyroDevLock(gyro);
|
||||||
if (gyro->dataReady == false) {
|
if (gyro->dataReady == false) {
|
||||||
|
@ -73,26 +73,26 @@ STATIC_UNIT_TESTED bool fakeGyroRead(gyroDev_t *gyro)
|
||||||
}
|
}
|
||||||
gyro->dataReady = false;
|
gyro->dataReady = false;
|
||||||
|
|
||||||
gyro->gyroADCRaw[X] = fakeGyroADC[X];
|
gyro->gyroADCRaw[X] = virtualGyroADC[X];
|
||||||
gyro->gyroADCRaw[Y] = fakeGyroADC[Y];
|
gyro->gyroADCRaw[Y] = virtualGyroADC[Y];
|
||||||
gyro->gyroADCRaw[Z] = fakeGyroADC[Z];
|
gyro->gyroADCRaw[Z] = virtualGyroADC[Z];
|
||||||
|
|
||||||
gyroDevUnLock(gyro);
|
gyroDevUnLock(gyro);
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
static bool fakeGyroReadTemperature(gyroDev_t *gyro, int16_t *temperatureData)
|
static bool virtualGyroReadTemperature(gyroDev_t *gyro, int16_t *temperatureData)
|
||||||
{
|
{
|
||||||
UNUSED(gyro);
|
UNUSED(gyro);
|
||||||
UNUSED(temperatureData);
|
UNUSED(temperatureData);
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool fakeGyroDetect(gyroDev_t *gyro)
|
bool virtualGyroDetect(gyroDev_t *gyro)
|
||||||
{
|
{
|
||||||
gyro->initFn = fakeGyroInit;
|
gyro->initFn = virtualGyroInit;
|
||||||
gyro->readFn = fakeGyroRead;
|
gyro->readFn = virtualGyroRead;
|
||||||
gyro->temperatureFn = fakeGyroReadTemperature;
|
gyro->temperatureFn = virtualGyroReadTemperature;
|
||||||
#if defined(SIMULATOR_BUILD)
|
#if defined(SIMULATOR_BUILD)
|
||||||
gyro->scale = GYRO_SCALE_2000DPS;
|
gyro->scale = GYRO_SCALE_2000DPS;
|
||||||
#else
|
#else
|
||||||
|
@ -100,17 +100,17 @@ bool fakeGyroDetect(gyroDev_t *gyro)
|
||||||
#endif
|
#endif
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
#endif // USE_FAKE_GYRO
|
#endif // USE_VIRTUAL_GYRO
|
||||||
|
|
||||||
|
|
||||||
#ifdef USE_FAKE_ACC
|
#ifdef USE_VIRTUAL_ACC
|
||||||
|
|
||||||
static int16_t fakeAccData[XYZ_AXIS_COUNT];
|
static int16_t virtualAccData[XYZ_AXIS_COUNT];
|
||||||
accDev_t *fakeAccDev;
|
accDev_t *virtualAccDev;
|
||||||
|
|
||||||
static void fakeAccInit(accDev_t *acc)
|
static void virtualAccInit(accDev_t *acc)
|
||||||
{
|
{
|
||||||
fakeAccDev = acc;
|
virtualAccDev = acc;
|
||||||
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
|
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
|
||||||
if (pthread_mutex_init(&acc->lock, NULL) != 0) {
|
if (pthread_mutex_init(&acc->lock, NULL) != 0) {
|
||||||
printf("Create acc lock error!\n");
|
printf("Create acc lock error!\n");
|
||||||
|
@ -118,20 +118,20 @@ static void fakeAccInit(accDev_t *acc)
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
void fakeAccSet(accDev_t *acc, int16_t x, int16_t y, int16_t z)
|
void virtualAccSet(accDev_t *acc, int16_t x, int16_t y, int16_t z)
|
||||||
{
|
{
|
||||||
accDevLock(acc);
|
accDevLock(acc);
|
||||||
|
|
||||||
fakeAccData[X] = x;
|
virtualAccData[X] = x;
|
||||||
fakeAccData[Y] = y;
|
virtualAccData[Y] = y;
|
||||||
fakeAccData[Z] = z;
|
virtualAccData[Z] = z;
|
||||||
|
|
||||||
acc->dataReady = true;
|
acc->dataReady = true;
|
||||||
|
|
||||||
accDevUnLock(acc);
|
accDevUnLock(acc);
|
||||||
}
|
}
|
||||||
|
|
||||||
static bool fakeAccRead(accDev_t *acc)
|
static bool virtualAccRead(accDev_t *acc)
|
||||||
{
|
{
|
||||||
accDevLock(acc);
|
accDevLock(acc);
|
||||||
if (acc->dataReady == false) {
|
if (acc->dataReady == false) {
|
||||||
|
@ -140,19 +140,19 @@ static bool fakeAccRead(accDev_t *acc)
|
||||||
}
|
}
|
||||||
acc->dataReady = false;
|
acc->dataReady = false;
|
||||||
|
|
||||||
acc->ADCRaw[X] = fakeAccData[X];
|
acc->ADCRaw[X] = virtualAccData[X];
|
||||||
acc->ADCRaw[Y] = fakeAccData[Y];
|
acc->ADCRaw[Y] = virtualAccData[Y];
|
||||||
acc->ADCRaw[Z] = fakeAccData[Z];
|
acc->ADCRaw[Z] = virtualAccData[Z];
|
||||||
|
|
||||||
accDevUnLock(acc);
|
accDevUnLock(acc);
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool fakeAccDetect(accDev_t *acc)
|
bool virtualAccDetect(accDev_t *acc)
|
||||||
{
|
{
|
||||||
acc->initFn = fakeAccInit;
|
acc->initFn = virtualAccInit;
|
||||||
acc->readFn = fakeAccRead;
|
acc->readFn = virtualAccRead;
|
||||||
acc->revisionCode = 0;
|
acc->revisionCode = 0;
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
#endif // USE_FAKE_ACC
|
#endif // USE_VIRTUAL_ACC
|
|
@ -21,11 +21,11 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
struct accDev_s;
|
struct accDev_s;
|
||||||
extern struct accDev_s *fakeAccDev;
|
extern struct accDev_s *virtualAccDev;
|
||||||
bool fakeAccDetect(struct accDev_s *acc);
|
bool virtualAccDetect(struct accDev_s *acc);
|
||||||
void fakeAccSet(struct accDev_s *acc, int16_t x, int16_t y, int16_t z);
|
void virtualAccSet(struct accDev_s *acc, int16_t x, int16_t y, int16_t z);
|
||||||
|
|
||||||
struct gyroDev_s;
|
struct gyroDev_s;
|
||||||
extern struct gyroDev_s *fakeGyroDev;
|
extern struct gyroDev_s *virtualGyroDev;
|
||||||
bool fakeGyroDetect(struct gyroDev_s *gyro);
|
bool virtualGyroDetect(struct gyroDev_s *gyro);
|
||||||
void fakeGyroSet(struct gyroDev_s *gyro, int16_t x, int16_t y, int16_t z);
|
void virtualGyroSet(struct gyroDev_s *gyro, int16_t x, int16_t y, int16_t z);
|
|
@ -23,63 +23,63 @@
|
||||||
|
|
||||||
#include "platform.h"
|
#include "platform.h"
|
||||||
|
|
||||||
#ifdef USE_FAKE_BARO
|
#ifdef USE_VIRTUAL_BARO
|
||||||
|
|
||||||
#include "common/utils.h"
|
#include "common/utils.h"
|
||||||
|
|
||||||
#include "barometer.h"
|
#include "barometer.h"
|
||||||
#include "barometer_fake.h"
|
#include "barometer_virtual.h"
|
||||||
|
|
||||||
|
|
||||||
static int32_t fakePressure;
|
static int32_t virtualPressure;
|
||||||
static int32_t fakeTemperature;
|
static int32_t virtualTemperature;
|
||||||
|
|
||||||
|
|
||||||
static void fakeBaroStart(baroDev_t *baro)
|
static void virtualBaroStart(baroDev_t *baro)
|
||||||
{
|
{
|
||||||
UNUSED(baro);
|
UNUSED(baro);
|
||||||
}
|
}
|
||||||
|
|
||||||
static bool fakeBaroReadGet(baroDev_t *baro)
|
static bool virtualBaroReadGet(baroDev_t *baro)
|
||||||
{
|
{
|
||||||
UNUSED(baro);
|
UNUSED(baro);
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
static void fakeBaroCalculate(int32_t *pressure, int32_t *temperature)
|
static void virtualBaroCalculate(int32_t *pressure, int32_t *temperature)
|
||||||
{
|
{
|
||||||
if (pressure)
|
if (pressure)
|
||||||
*pressure = fakePressure;
|
*pressure = virtualPressure;
|
||||||
if (temperature)
|
if (temperature)
|
||||||
*temperature = fakeTemperature;
|
*temperature = virtualTemperature;
|
||||||
}
|
}
|
||||||
|
|
||||||
void fakeBaroSet(int32_t pressure, int32_t temperature)
|
void virtualBaroSet(int32_t pressure, int32_t temperature)
|
||||||
{
|
{
|
||||||
fakePressure = pressure;
|
virtualPressure = pressure;
|
||||||
fakeTemperature = temperature;
|
virtualTemperature = temperature;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool fakeBaroDetect(baroDev_t *baro)
|
bool virtualBaroDetect(baroDev_t *baro)
|
||||||
{
|
{
|
||||||
fakePressure = 101325; // pressure in Pa (0m MSL)
|
virtualPressure = 101325; // pressure in Pa (0m MSL)
|
||||||
fakeTemperature = 2500; // temperature in 0.01 C = 25 deg
|
virtualTemperature = 2500; // temperature in 0.01 C = 25 deg
|
||||||
|
|
||||||
// these are dummy as temperature is measured as part of pressure
|
// these are dummy as temperature is measured as part of pressure
|
||||||
baro->combined_read = true;
|
baro->combined_read = true;
|
||||||
baro->ut_delay = 10000;
|
baro->ut_delay = 10000;
|
||||||
baro->get_ut = fakeBaroReadGet;
|
baro->get_ut = virtualBaroReadGet;
|
||||||
baro->read_ut = fakeBaroReadGet;
|
baro->read_ut = virtualBaroReadGet;
|
||||||
baro->start_ut = fakeBaroStart;
|
baro->start_ut = virtualBaroStart;
|
||||||
|
|
||||||
// only _up part is executed, and gets both temperature and pressure
|
// only _up part is executed, and gets both temperature and pressure
|
||||||
baro->up_delay = 10000;
|
baro->up_delay = 10000;
|
||||||
baro->start_up = fakeBaroStart;
|
baro->start_up = virtualBaroStart;
|
||||||
baro->read_up = fakeBaroReadGet;
|
baro->read_up = virtualBaroReadGet;
|
||||||
baro->get_up = fakeBaroReadGet;
|
baro->get_up = virtualBaroReadGet;
|
||||||
baro->calculate = fakeBaroCalculate;
|
baro->calculate = virtualBaroCalculate;
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
#endif // USE_FAKE_BARO
|
#endif // USE_VIRTUAL_BARO
|
|
@ -21,5 +21,5 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
struct baroDev_s;
|
struct baroDev_s;
|
||||||
bool fakeBaroDetect(struct baroDev_s *baro);
|
bool virtualBaroDetect(struct baroDev_s *baro);
|
||||||
void fakeBaroSet(int32_t pressure, int32_t temperature);
|
void virtualBaroSet(int32_t pressure, int32_t temperature);
|
|
@ -23,7 +23,7 @@
|
||||||
|
|
||||||
#include "platform.h"
|
#include "platform.h"
|
||||||
|
|
||||||
#ifdef USE_FAKE_MAG
|
#ifdef USE_VIRTUAL_MAG
|
||||||
|
|
||||||
#include "build/build_config.h"
|
#include "build/build_config.h"
|
||||||
|
|
||||||
|
@ -31,44 +31,44 @@
|
||||||
#include "common/utils.h"
|
#include "common/utils.h"
|
||||||
|
|
||||||
#include "compass.h"
|
#include "compass.h"
|
||||||
#include "compass_fake.h"
|
#include "compass_virtual.h"
|
||||||
|
|
||||||
|
|
||||||
static int16_t fakeMagData[XYZ_AXIS_COUNT];
|
static int16_t virtualMagData[XYZ_AXIS_COUNT];
|
||||||
|
|
||||||
static bool fakeMagInit(magDev_t *mag)
|
static bool virtualMagInit(magDev_t *mag)
|
||||||
{
|
{
|
||||||
UNUSED(mag);
|
UNUSED(mag);
|
||||||
|
|
||||||
// initially point north
|
// initially point north
|
||||||
fakeMagData[X] = 4096;
|
virtualMagData[X] = 4096;
|
||||||
fakeMagData[Y] = 0;
|
virtualMagData[Y] = 0;
|
||||||
fakeMagData[Z] = 0;
|
virtualMagData[Z] = 0;
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
void fakeMagSet(int16_t x, int16_t y, int16_t z)
|
void virtualMagSet(int16_t x, int16_t y, int16_t z)
|
||||||
{
|
{
|
||||||
fakeMagData[X] = x;
|
virtualMagData[X] = x;
|
||||||
fakeMagData[Y] = y;
|
virtualMagData[Y] = y;
|
||||||
fakeMagData[Z] = z;
|
virtualMagData[Z] = z;
|
||||||
}
|
}
|
||||||
|
|
||||||
static bool fakeMagRead(magDev_t *mag, int16_t *magData)
|
static bool virtualMagRead(magDev_t *mag, int16_t *magData)
|
||||||
{
|
{
|
||||||
UNUSED(mag);
|
UNUSED(mag);
|
||||||
|
|
||||||
magData[X] = fakeMagData[X];
|
magData[X] = virtualMagData[X];
|
||||||
magData[Y] = fakeMagData[Y];
|
magData[Y] = virtualMagData[Y];
|
||||||
magData[Z] = fakeMagData[Z];
|
magData[Z] = virtualMagData[Z];
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool fakeMagDetect(magDev_t *mag)
|
bool virtualMagDetect(magDev_t *mag)
|
||||||
{
|
{
|
||||||
mag->init = fakeMagInit;
|
mag->init = virtualMagInit;
|
||||||
mag->read = fakeMagRead;
|
mag->read = virtualMagRead;
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
#endif // USE_FAKE_MAG
|
#endif // USE_VIRTUAL_MAG
|
||||||
|
|
|
@ -21,5 +21,5 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
struct magDev_s;
|
struct magDev_s;
|
||||||
bool fakeMagDetect(struct magDev_s *mag);
|
bool virtualMagDetect(struct magDev_s *mag);
|
||||||
void fakeMagSet(int16_t x, int16_t y, int16_t z);
|
void virtualMagSet(int16_t x, int16_t y, int16_t z);
|
|
@ -34,7 +34,7 @@ typedef struct statusLedConfig_s {
|
||||||
PG_DECLARE(statusLedConfig_t, statusLedConfig);
|
PG_DECLARE(statusLedConfig_t, statusLedConfig);
|
||||||
|
|
||||||
// Helpful macros
|
// Helpful macros
|
||||||
#if defined(UNIT_TEST) || defined(USE_FAKE_LED)
|
#if defined(UNIT_TEST) || defined(USE_VIRTUAL_LED)
|
||||||
|
|
||||||
#define LED0_TOGGLE NOOP
|
#define LED0_TOGGLE NOOP
|
||||||
#define LED0_OFF NOOP
|
#define LED0_OFF NOOP
|
||||||
|
|
|
@ -408,7 +408,7 @@ void init(void)
|
||||||
targetPreInit();
|
targetPreInit();
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#if !defined(USE_FAKE_LED)
|
#if !defined(USE_VIRTUAL_LED)
|
||||||
ledInit(statusLedConfig());
|
ledInit(statusLedConfig());
|
||||||
#endif
|
#endif
|
||||||
LED2_ON;
|
LED2_ON;
|
||||||
|
|
|
@ -42,7 +42,7 @@
|
||||||
#include "io/serial_4way_impl.h"
|
#include "io/serial_4way_impl.h"
|
||||||
#include "io/serial_4way_avrootloader.h"
|
#include "io/serial_4way_avrootloader.h"
|
||||||
|
|
||||||
#if defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) && !defined(USE_FAKE_ESC)
|
#if defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) && !defined(USE_VIRTUAL_ESC)
|
||||||
|
|
||||||
// Bootloader commands
|
// Bootloader commands
|
||||||
// RunCmd
|
// RunCmd
|
||||||
|
@ -344,12 +344,12 @@ uint8_t BL_VerifyFlash(ioMem_t *pMem)
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
#if defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) && defined(USE_FAKE_ESC)
|
#if defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) && defined(USE_VIRTUAL_ESC)
|
||||||
|
|
||||||
#define FAKE_PAGE_SIZE 512
|
#define VIRTUAL_PAGE_SIZE 512
|
||||||
#define FAKE_FLASH_SIZE 16385
|
#define VIRTUAL_FLASH_SIZE 16385
|
||||||
|
|
||||||
static uint8_t fakeFlash[FAKE_FLASH_SIZE] =
|
static uint8_t virtualFlash[VIRTUAL_FLASH_SIZE] =
|
||||||
{
|
{
|
||||||
0x02, 0x19, 0xFD, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x02, 0x00, 0xAA, 0xFF, 0xFF,
|
0x02, 0x19, 0xFD, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x02, 0x00, 0xAA, 0xFF, 0xFF,
|
||||||
0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
|
0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
|
||||||
|
@ -858,7 +858,7 @@ static uint8_t BL_ReadA(uint8_t cmd, ioMem_t *pMem)
|
||||||
uint16_t bytes = pMem->D_NUM_BYTES;
|
uint16_t bytes = pMem->D_NUM_BYTES;
|
||||||
if (bytes == 0) bytes = 256;
|
if (bytes == 0) bytes = 256;
|
||||||
|
|
||||||
memcpy(pMem->D_PTR_I, &fakeFlash[address], bytes);
|
memcpy(pMem->D_PTR_I, &virtualFlash[address], bytes);
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -870,7 +870,7 @@ static uint8_t BL_WriteA(uint8_t cmd, ioMem_t *pMem, uint32_t timeout)
|
||||||
|
|
||||||
uint16_t bytes = pMem->D_NUM_BYTES;
|
uint16_t bytes = pMem->D_NUM_BYTES;
|
||||||
if (bytes == 0) bytes = 256;
|
if (bytes == 0) bytes = 256;
|
||||||
memcpy(&fakeFlash[address], pMem->D_PTR_I, bytes);
|
memcpy(&virtualFlash[address], pMem->D_PTR_I, bytes);
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -888,9 +888,9 @@ uint8_t BL_ReadEEprom(ioMem_t *pMem)
|
||||||
uint8_t BL_PageErase(ioMem_t *pMem)
|
uint8_t BL_PageErase(ioMem_t *pMem)
|
||||||
{
|
{
|
||||||
uint16_t address = pMem->D_FLASH_ADDR_H << 8 | pMem->D_FLASH_ADDR_L;
|
uint16_t address = pMem->D_FLASH_ADDR_H << 8 | pMem->D_FLASH_ADDR_L;
|
||||||
if (address + FAKE_PAGE_SIZE > FAKE_FLASH_SIZE)
|
if (address + VIRTUAL_PAGE_SIZE > VIRTUAL_FLASH_SIZE)
|
||||||
return false;
|
return false;
|
||||||
memset(&fakeFlash[address], 0xFF, FAKE_PAGE_SIZE);
|
memset(&virtualFlash[address], 0xFF, VIRTUAL_PAGE_SIZE);
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -20,7 +20,7 @@
|
||||||
|
|
||||||
#include "platform.h"
|
#include "platform.h"
|
||||||
#ifdef USE_SERIALRX
|
#ifdef USE_SERIALRX
|
||||||
#if defined(USE_SPEKTRUM_REAL_RSSI) || defined(USE_SPEKTRUM_FAKE_RSSI)
|
#if defined(USE_SPEKTRUM_REAL_RSSI) || defined(USE_SPEKTRUM_VIRTUAL_RSSI)
|
||||||
|
|
||||||
#include "config/feature.h"
|
#include "config/feature.h"
|
||||||
#include "common/utils.h"
|
#include "common/utils.h"
|
||||||
|
@ -38,7 +38,7 @@
|
||||||
// Number of fade outs counted as a link loss when using USE_SPEKTRUM_REAL_RSSI
|
// Number of fade outs counted as a link loss when using USE_SPEKTRUM_REAL_RSSI
|
||||||
#define SPEKTRUM_RSSI_LINK_LOSS_FADES 5
|
#define SPEKTRUM_RSSI_LINK_LOSS_FADES 5
|
||||||
|
|
||||||
#ifdef USE_SPEKTRUM_FAKE_RSSI
|
#ifdef USE_SPEKTRUM_VIRTUAL_RSSI
|
||||||
// Spektrum Rx type. Determined by bind method.
|
// Spektrum Rx type. Determined by bind method.
|
||||||
static bool spektrumSatInternal = true; // Assume internal,bound by BF.
|
static bool spektrumSatInternal = true; // Assume internal,bound by BF.
|
||||||
|
|
||||||
|
@ -161,14 +161,14 @@ void spektrumHandleRSSI(volatile uint8_t spekFrame[])
|
||||||
spek_last_rssi = rssi;
|
spek_last_rssi = rssi;
|
||||||
}
|
}
|
||||||
|
|
||||||
#ifdef USE_SPEKTRUM_FAKE_RSSI
|
#ifdef USE_SPEKTRUM_VIRTUAL_RSSI
|
||||||
else
|
else
|
||||||
#endif
|
#endif
|
||||||
#endif // USE_SPEKTRUM_REAL_RSSI
|
#endif // USE_SPEKTRUM_REAL_RSSI
|
||||||
|
|
||||||
#ifdef USE_SPEKTRUM_FAKE_RSSI
|
#ifdef USE_SPEKTRUM_VIRTUAL_RSSI
|
||||||
{
|
{
|
||||||
// Fake RSSI value computed from fades
|
// Virtual RSSI value computed from fades
|
||||||
|
|
||||||
const uint32_t current_secs = micros() / 1000 / (1000 / SPEKTRUM_FADE_REPORTS_PER_SEC);
|
const uint32_t current_secs = micros() / 1000 / (1000 / SPEKTRUM_FADE_REPORTS_PER_SEC);
|
||||||
uint16_t fade;
|
uint16_t fade;
|
||||||
|
@ -210,7 +210,7 @@ void spektrumHandleRSSI(volatile uint8_t spekFrame[])
|
||||||
spek_fade_last_sec = current_secs;
|
spek_fade_last_sec = current_secs;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
#endif // USE_SPEKTRUM_FAKE_RSSI
|
#endif // USE_SPEKTRUM_VIRTUAL_RSSI
|
||||||
}
|
}
|
||||||
#endif // USE_SPEKTRUM_REAL_RSSI || USE_SPEKTRUM_FAKE_RSSI
|
#endif // USE_SPEKTRUM_REAL_RSSI || USE_SPEKTRUM_VIRTUAL_RSSI
|
||||||
#endif // USE_SERIALRX
|
#endif // USE_SERIALRX
|
||||||
|
|
|
@ -121,7 +121,7 @@ static uint8_t spektrumFrameStatus(rxRuntimeState_t *rxRuntimeState)
|
||||||
if (rcFrameComplete) {
|
if (rcFrameComplete) {
|
||||||
rcFrameComplete = false;
|
rcFrameComplete = false;
|
||||||
|
|
||||||
#if defined(USE_SPEKTRUM_REAL_RSSI) || defined(USE_SPEKTRUM_FAKE_RSSI)
|
#if defined(USE_SPEKTRUM_REAL_RSSI) || defined(USE_SPEKTRUM_VIRTUAL_RSSI)
|
||||||
spektrumHandleRSSI(spekFrame);
|
spektrumHandleRSSI(spekFrame);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
|
@ -47,7 +47,7 @@ typedef enum {
|
||||||
ACC_BMI160,
|
ACC_BMI160,
|
||||||
ACC_BMI270,
|
ACC_BMI270,
|
||||||
ACC_LSM6DSO,
|
ACC_LSM6DSO,
|
||||||
ACC_FAKE
|
ACC_VIRTUAL
|
||||||
} accelerationSensor_e;
|
} accelerationSensor_e;
|
||||||
|
|
||||||
typedef struct acc_s {
|
typedef struct acc_s {
|
||||||
|
|
|
@ -37,7 +37,7 @@
|
||||||
#include "config/feature.h"
|
#include "config/feature.h"
|
||||||
|
|
||||||
#include "drivers/accgyro/accgyro.h"
|
#include "drivers/accgyro/accgyro.h"
|
||||||
#include "drivers/accgyro/accgyro_fake.h"
|
#include "drivers/accgyro/accgyro_virtual.h"
|
||||||
#include "drivers/accgyro/accgyro_mpu.h"
|
#include "drivers/accgyro/accgyro_mpu.h"
|
||||||
#include "drivers/accgyro/accgyro_mpu3050.h"
|
#include "drivers/accgyro/accgyro_mpu3050.h"
|
||||||
#include "drivers/accgyro/accgyro_mpu6050.h"
|
#include "drivers/accgyro/accgyro_mpu6050.h"
|
||||||
|
@ -91,7 +91,7 @@
|
||||||
!defined(USE_ACC_SPI_ICM20689) && !defined(USE_ACCGYRO_LSM6DSO) && !defined(USE_ACCGYRO_BMI160) && \
|
!defined(USE_ACC_SPI_ICM20689) && !defined(USE_ACCGYRO_LSM6DSO) && !defined(USE_ACCGYRO_BMI160) && \
|
||||||
!defined(USE_ACCGYRO_BMI270) && !defined(USE_ACC_SPI_ICM42605) && !defined(USE_ACC_SPI_ICM42688P) && \
|
!defined(USE_ACCGYRO_BMI270) && !defined(USE_ACC_SPI_ICM42605) && !defined(USE_ACC_SPI_ICM42688P) && \
|
||||||
!defined(USE_ACC_ADXL345) && !defined(USE_ACC_BMA280) && !defined(USE_ACC_LSM303DLHC) && \
|
!defined(USE_ACC_ADXL345) && !defined(USE_ACC_BMA280) && !defined(USE_ACC_LSM303DLHC) && \
|
||||||
!defined(USE_ACC_MMA8452) && !defined(USE_FAKE_ACC)
|
!defined(USE_ACC_MMA8452) && !defined(USE_VIRTUAL_ACC)
|
||||||
#error At least one USE_ACC device definition required
|
#error At least one USE_ACC device definition required
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
@ -328,10 +328,10 @@ retry:
|
||||||
FALLTHROUGH;
|
FALLTHROUGH;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef USE_FAKE_ACC
|
#ifdef USE_VIRTUAL_ACC
|
||||||
case ACC_FAKE:
|
case ACC_VIRTUAL:
|
||||||
if (fakeAccDetect(dev)) {
|
if (virtualAccDetect(dev)) {
|
||||||
accHardware = ACC_FAKE;
|
accHardware = ACC_VIRTUAL;
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
FALLTHROUGH;
|
FALLTHROUGH;
|
||||||
|
|
|
@ -40,7 +40,7 @@
|
||||||
#include "drivers/barometer/barometer_bmp388.h"
|
#include "drivers/barometer/barometer_bmp388.h"
|
||||||
#include "drivers/barometer/barometer_dps310.h"
|
#include "drivers/barometer/barometer_dps310.h"
|
||||||
#include "drivers/barometer/barometer_qmp6988.h"
|
#include "drivers/barometer/barometer_qmp6988.h"
|
||||||
#include "drivers/barometer/barometer_fake.h"
|
#include "drivers/barometer/barometer_virtual.h"
|
||||||
#include "drivers/barometer/barometer_ms5611.h"
|
#include "drivers/barometer/barometer_ms5611.h"
|
||||||
#include "drivers/barometer/barometer_lps.h"
|
#include "drivers/barometer/barometer_lps.h"
|
||||||
#include "drivers/barometer/barometer_2smpb_02b.h"
|
#include "drivers/barometer/barometer_2smpb_02b.h"
|
||||||
|
@ -185,7 +185,7 @@ static bool baroDetect(baroDev_t *baroDev, baroSensor_e baroHardwareToUse)
|
||||||
UNUSED(dev);
|
UNUSED(dev);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifndef USE_FAKE_BARO
|
#ifndef USE_VIRTUAL_BARO
|
||||||
switch (barometerConfig()->baro_busType) {
|
switch (barometerConfig()->baro_busType) {
|
||||||
#ifdef USE_I2C
|
#ifdef USE_I2C
|
||||||
case BUS_TYPE_I2C:
|
case BUS_TYPE_I2C:
|
||||||
|
@ -208,7 +208,7 @@ static bool baroDetect(baroDev_t *baroDev, baroSensor_e baroHardwareToUse)
|
||||||
default:
|
default:
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
#endif // USE_FAKE_BARO
|
#endif // USE_VIRTUAL_BARO
|
||||||
|
|
||||||
switch (baroHardware) {
|
switch (baroHardware) {
|
||||||
case BARO_DEFAULT:
|
case BARO_DEFAULT:
|
||||||
|
@ -304,10 +304,10 @@ static bool baroDetect(baroDev_t *baroDev, baroSensor_e baroHardwareToUse)
|
||||||
#endif
|
#endif
|
||||||
FALLTHROUGH;
|
FALLTHROUGH;
|
||||||
|
|
||||||
case BARO_FAKE:
|
case BARO_VIRTUAL:
|
||||||
#ifdef USE_FAKE_BARO
|
#ifdef USE_VIRTUAL_BARO
|
||||||
if (fakeBaroDetect(baroDev)) {
|
if (virtualBaroDetect(baroDev)) {
|
||||||
baroHardware = BARO_FAKE;
|
baroHardware = BARO_VIRTUAL;
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
@ -329,10 +329,10 @@ static bool baroDetect(baroDev_t *baroDev, baroSensor_e baroHardwareToUse)
|
||||||
|
|
||||||
void baroInit(void)
|
void baroInit(void)
|
||||||
{
|
{
|
||||||
#ifndef USE_FAKE_BARO
|
#ifndef USE_VIRTUAL_BARO
|
||||||
baroReady = baroDetect(&baro.dev, barometerConfig()->baro_hardware);
|
baroReady = baroDetect(&baro.dev, barometerConfig()->baro_hardware);
|
||||||
#else
|
#else
|
||||||
baroReady = baroDetect(&baro.dev, BARO_FAKE);
|
baroReady = baroDetect(&baro.dev, BARO_VIRTUAL);
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -34,7 +34,7 @@ typedef enum {
|
||||||
BARO_BMP388 = 7,
|
BARO_BMP388 = 7,
|
||||||
BARO_DPS310 = 8,
|
BARO_DPS310 = 8,
|
||||||
BARO_2SMPB_02B = 9,
|
BARO_2SMPB_02B = 9,
|
||||||
BARO_FAKE = 10,
|
BARO_VIRTUAL = 10,
|
||||||
} baroSensor_e;
|
} baroSensor_e;
|
||||||
|
|
||||||
typedef struct barometerConfig_s {
|
typedef struct barometerConfig_s {
|
||||||
|
|
|
@ -37,7 +37,7 @@
|
||||||
#include "drivers/compass/compass.h"
|
#include "drivers/compass/compass.h"
|
||||||
#include "drivers/compass/compass_ak8975.h"
|
#include "drivers/compass/compass_ak8975.h"
|
||||||
#include "drivers/compass/compass_ak8963.h"
|
#include "drivers/compass/compass_ak8963.h"
|
||||||
#include "drivers/compass/compass_fake.h"
|
#include "drivers/compass/compass_virtual.h"
|
||||||
#include "drivers/compass/compass_hmc5883l.h"
|
#include "drivers/compass/compass_hmc5883l.h"
|
||||||
#include "drivers/compass/compass_lis3mdl.h"
|
#include "drivers/compass/compass_lis3mdl.h"
|
||||||
#include "drivers/compass/compass_mpu925x_ak8963.h"
|
#include "drivers/compass/compass_mpu925x_ak8963.h"
|
||||||
|
|
|
@ -173,8 +173,8 @@ static bool isOnFirstGyroCalibrationCycle(const gyroCalibration_t *gyroCalibrati
|
||||||
|
|
||||||
static void gyroSetCalibrationCycles(gyroSensor_t *gyroSensor)
|
static void gyroSetCalibrationCycles(gyroSensor_t *gyroSensor)
|
||||||
{
|
{
|
||||||
#if defined(USE_FAKE_GYRO) && !defined(UNIT_TEST)
|
#if defined(USE_VIRTUAL_GYRO) && !defined(UNIT_TEST)
|
||||||
if (gyroSensor->gyroDev.gyroHardware == GYRO_FAKE) {
|
if (gyroSensor->gyroDev.gyroHardware == GYRO_VIRTUAL) {
|
||||||
gyroSensor->calibration.cyclesRemaining = 0;
|
gyroSensor->calibration.cyclesRemaining = 0;
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
|
@ -35,7 +35,7 @@
|
||||||
#include "config/config.h"
|
#include "config/config.h"
|
||||||
|
|
||||||
#include "drivers/accgyro/accgyro.h"
|
#include "drivers/accgyro/accgyro.h"
|
||||||
#include "drivers/accgyro/accgyro_fake.h"
|
#include "drivers/accgyro/accgyro_virtual.h"
|
||||||
#include "drivers/accgyro/accgyro_mpu.h"
|
#include "drivers/accgyro/accgyro_mpu.h"
|
||||||
#include "drivers/accgyro/accgyro_mpu3050.h"
|
#include "drivers/accgyro/accgyro_mpu3050.h"
|
||||||
#include "drivers/accgyro/accgyro_mpu6050.h"
|
#include "drivers/accgyro/accgyro_mpu6050.h"
|
||||||
|
@ -75,7 +75,7 @@
|
||||||
#if !defined(USE_GYRO_L3G4200D) && !defined(USE_GYRO_MPU3050) && !defined(USE_GYRO_MPU6050) && \
|
#if !defined(USE_GYRO_L3G4200D) && !defined(USE_GYRO_MPU3050) && !defined(USE_GYRO_MPU6050) && \
|
||||||
!defined(USE_GYRO_MPU6500) && !defined(USE_GYRO_SPI_ICM20689) && !defined(USE_GYRO_SPI_MPU6000) && \
|
!defined(USE_GYRO_MPU6500) && !defined(USE_GYRO_SPI_ICM20689) && !defined(USE_GYRO_SPI_MPU6000) && \
|
||||||
!defined(USE_GYRO_SPI_MPU6500) && !defined(USE_GYRO_SPI_MPU9250) && !defined(USE_GYRO_L3GD20) && \
|
!defined(USE_GYRO_SPI_MPU6500) && !defined(USE_GYRO_SPI_MPU9250) && !defined(USE_GYRO_L3GD20) && \
|
||||||
!defined(USE_GYRO_SPI_ICM42605) && !defined(USE_GYRO_SPI_ICM42688P) && !defined(USE_ACCGYRO_BMI270) && !defined(USE_FAKE_GYRO)
|
!defined(USE_GYRO_SPI_ICM42605) && !defined(USE_GYRO_SPI_ICM42688P) && !defined(USE_ACCGYRO_BMI270) && !defined(USE_VIRTUAL_GYRO)
|
||||||
#error At least one USE_GYRO device definition required
|
#error At least one USE_GYRO device definition required
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
@ -316,7 +316,7 @@ void gyroInitSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t *config)
|
||||||
switch (gyroSensor->gyroDev.gyroHardware) {
|
switch (gyroSensor->gyroDev.gyroHardware) {
|
||||||
case GYRO_NONE: // Won't ever actually get here, but included to account for all gyro types
|
case GYRO_NONE: // Won't ever actually get here, but included to account for all gyro types
|
||||||
case GYRO_DEFAULT:
|
case GYRO_DEFAULT:
|
||||||
case GYRO_FAKE:
|
case GYRO_VIRTUAL:
|
||||||
case GYRO_MPU6050:
|
case GYRO_MPU6050:
|
||||||
case GYRO_L3G4200D:
|
case GYRO_L3G4200D:
|
||||||
case GYRO_MPU3050:
|
case GYRO_MPU3050:
|
||||||
|
@ -504,10 +504,10 @@ STATIC_UNIT_TESTED gyroHardware_e gyroDetect(gyroDev_t *dev)
|
||||||
FALLTHROUGH;
|
FALLTHROUGH;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef USE_FAKE_GYRO
|
#ifdef USE_VIRTUAL_GYRO
|
||||||
case GYRO_FAKE:
|
case GYRO_VIRTUAL:
|
||||||
if (fakeGyroDetect(dev)) {
|
if (virtualGyroDetect(dev)) {
|
||||||
gyroHardware = GYRO_FAKE;
|
gyroHardware = GYRO_VIRTUAL;
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
FALLTHROUGH;
|
FALLTHROUGH;
|
||||||
|
@ -533,7 +533,7 @@ static bool gyroDetectSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t
|
||||||
|
|
||||||
bool gyroFound = mpuDetect(&gyroSensor->gyroDev, config);
|
bool gyroFound = mpuDetect(&gyroSensor->gyroDev, config);
|
||||||
|
|
||||||
#if !defined(USE_FAKE_GYRO) // Allow resorting to fake accgyro if defined
|
#if !defined(USE_VIRTUAL_GYRO) // Allow resorting to virtual accgyro if defined
|
||||||
if (!gyroFound) {
|
if (!gyroFound) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
|
@ -34,7 +34,7 @@
|
||||||
#define HANG_ON_ERRORS
|
#define HANG_ON_ERRORS
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#define USE_FAKE_GYRO
|
#define USE_VIRTUAL_GYRO
|
||||||
|
|
||||||
#define USE_UART1
|
#define USE_UART1
|
||||||
#define USE_UART2
|
#define USE_UART2
|
||||||
|
|
|
@ -41,8 +41,8 @@
|
||||||
#include "drivers/timer.h"
|
#include "drivers/timer.h"
|
||||||
#include "timer_def.h"
|
#include "timer_def.h"
|
||||||
|
|
||||||
#include "drivers/accgyro/accgyro_fake.h"
|
#include "drivers/accgyro/accgyro_virtual.h"
|
||||||
#include "drivers/barometer/barometer_fake.h"
|
#include "drivers/barometer/barometer_virtual.h"
|
||||||
#include "flight/imu.h"
|
#include "flight/imu.h"
|
||||||
|
|
||||||
#include "config/feature.h"
|
#include "config/feature.h"
|
||||||
|
@ -135,17 +135,17 @@ void updateState(const fdm_packet* pkt)
|
||||||
x = constrain(-pkt->imu_linear_acceleration_xyz[0] * ACC_SCALE, -32767, 32767);
|
x = constrain(-pkt->imu_linear_acceleration_xyz[0] * ACC_SCALE, -32767, 32767);
|
||||||
y = constrain(-pkt->imu_linear_acceleration_xyz[1] * ACC_SCALE, -32767, 32767);
|
y = constrain(-pkt->imu_linear_acceleration_xyz[1] * ACC_SCALE, -32767, 32767);
|
||||||
z = constrain(-pkt->imu_linear_acceleration_xyz[2] * ACC_SCALE, -32767, 32767);
|
z = constrain(-pkt->imu_linear_acceleration_xyz[2] * ACC_SCALE, -32767, 32767);
|
||||||
fakeAccSet(fakeAccDev, x, y, z);
|
virtualAccSet(virtualAccDev, 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]);
|
// 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 = constrain(pkt->imu_angular_velocity_rpy[0] * GYRO_SCALE * RAD2DEG, -32767, 32767);
|
x = constrain(pkt->imu_angular_velocity_rpy[0] * GYRO_SCALE * RAD2DEG, -32767, 32767);
|
||||||
y = constrain(-pkt->imu_angular_velocity_rpy[1] * GYRO_SCALE * RAD2DEG, -32767, 32767);
|
y = constrain(-pkt->imu_angular_velocity_rpy[1] * GYRO_SCALE * RAD2DEG, -32767, 32767);
|
||||||
z = constrain(-pkt->imu_angular_velocity_rpy[2] * GYRO_SCALE * RAD2DEG, -32767, 32767);
|
z = constrain(-pkt->imu_angular_velocity_rpy[2] * GYRO_SCALE * RAD2DEG, -32767, 32767);
|
||||||
fakeGyroSet(fakeGyroDev, x, y, z);
|
virtualGyroSet(virtualGyroDev, 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]);
|
// printf("[gyr]%lf,%lf,%lf\n", pkt->imu_angular_velocity_rpy[0], pkt->imu_angular_velocity_rpy[1], pkt->imu_angular_velocity_rpy[2]);
|
||||||
|
|
||||||
// temperature in 0.01 C = 25 deg
|
// temperature in 0.01 C = 25 deg
|
||||||
fakeBaroSet(pkt->pressure, 2500);
|
virtualBaroSet(pkt->pressure, 2500);
|
||||||
#if !defined(USE_IMU_CALC)
|
#if !defined(USE_IMU_CALC)
|
||||||
#if defined(SET_IMU_FROM_EULER)
|
#if defined(SET_IMU_FROM_EULER)
|
||||||
// set from Euler
|
// set from Euler
|
||||||
|
@ -288,7 +288,7 @@ void systemInit(void)
|
||||||
clock_gettime(CLOCK_MONOTONIC, &start_time);
|
clock_gettime(CLOCK_MONOTONIC, &start_time);
|
||||||
printf("[system]Init...\n");
|
printf("[system]Init...\n");
|
||||||
|
|
||||||
SystemCoreClock = 500 * 1e6; // fake 500MHz
|
SystemCoreClock = 500 * 1e6; // virtual 500MHz
|
||||||
|
|
||||||
if (pthread_mutex_init(&updateLock, NULL) != 0) {
|
if (pthread_mutex_init(&updateLock, NULL) != 0) {
|
||||||
printf("Create updateLock error!\n");
|
printf("Create updateLock error!\n");
|
||||||
|
@ -658,7 +658,7 @@ uint16_t adcGetChannel(uint8_t channel)
|
||||||
char _estack;
|
char _estack;
|
||||||
char _Min_Stack_Size;
|
char _Min_Stack_Size;
|
||||||
|
|
||||||
// fake EEPROM
|
// virtual EEPROM
|
||||||
static FILE *eepromFd = NULL;
|
static FILE *eepromFd = NULL;
|
||||||
|
|
||||||
void FLASH_Unlock(void)
|
void FLASH_Unlock(void)
|
||||||
|
|
|
@ -67,19 +67,19 @@
|
||||||
#undef SCHEDULER_DELAY_LIMIT
|
#undef SCHEDULER_DELAY_LIMIT
|
||||||
#define SCHEDULER_DELAY_LIMIT 1
|
#define SCHEDULER_DELAY_LIMIT 1
|
||||||
|
|
||||||
#define USE_FAKE_LED
|
#define USE_VIRTUAL_LED
|
||||||
|
|
||||||
#define USE_ACC
|
#define USE_ACC
|
||||||
#define USE_FAKE_ACC
|
#define USE_VIRTUAL_ACC
|
||||||
|
|
||||||
#define USE_GYRO
|
#define USE_GYRO
|
||||||
#define USE_FAKE_GYRO
|
#define USE_VIRTUAL_GYRO
|
||||||
|
|
||||||
#define USE_MAG
|
#define USE_MAG
|
||||||
#define USE_FAKE_MAG
|
#define USE_VIRTUAL_MAG
|
||||||
|
|
||||||
#define USE_BARO
|
#define USE_BARO
|
||||||
#define USE_FAKE_BARO
|
#define USE_VIRTUAL_BARO
|
||||||
|
|
||||||
#define USABLE_TIMER_CHANNEL_COUNT 0
|
#define USABLE_TIMER_CHANNEL_COUNT 0
|
||||||
|
|
||||||
|
|
|
@ -3,7 +3,7 @@ TARGET_MCU_FAMILY := SITL
|
||||||
SIMULATOR_BUILD = yes
|
SIMULATOR_BUILD = yes
|
||||||
|
|
||||||
TARGET_SRC = \
|
TARGET_SRC = \
|
||||||
drivers/accgyro/accgyro_fake.c \
|
drivers/accgyro/accgyro_virtual.c \
|
||||||
drivers/barometer/barometer_fake.c \
|
drivers/barometer/barometer_virtual.c \
|
||||||
drivers/compass/compass_fake.c \
|
drivers/compass/compass_virtual.c \
|
||||||
drivers/serial_tcp.c
|
drivers/serial_tcp.c
|
||||||
|
|
|
@ -85,7 +85,7 @@
|
||||||
In the future we can move to specific drivers being added only - to save flash space.
|
In the future we can move to specific drivers being added only - to save flash space.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#if defined(USE_MAG) && !defined(USE_FAKE_MAG)
|
#if defined(USE_MAG) && !defined(USE_VIRTUAL_MAG)
|
||||||
#define USE_MAG_DATA_READY_SIGNAL
|
#define USE_MAG_DATA_READY_SIGNAL
|
||||||
#define USE_MAG_HMC5883
|
#define USE_MAG_HMC5883
|
||||||
#define USE_MAG_SPI_HMC5883
|
#define USE_MAG_SPI_HMC5883
|
||||||
|
@ -235,7 +235,7 @@
|
||||||
#undef USE_SPEKTRUM_BIND
|
#undef USE_SPEKTRUM_BIND
|
||||||
#undef USE_SPEKTRUM_BIND_PLUG
|
#undef USE_SPEKTRUM_BIND_PLUG
|
||||||
#undef USE_SPEKTRUM_REAL_RSSI
|
#undef USE_SPEKTRUM_REAL_RSSI
|
||||||
#undef USE_SPEKTRUM_FAKE_RSSI
|
#undef USE_SPEKTRUM_VIRTUAL_RSSI
|
||||||
#undef USE_SPEKTRUM_RSSI_PERCENT_CONVERSION
|
#undef USE_SPEKTRUM_RSSI_PERCENT_CONVERSION
|
||||||
#undef USE_SPEKTRUM_VTX_CONTROL
|
#undef USE_SPEKTRUM_VTX_CONTROL
|
||||||
#undef USE_SPEKTRUM_VTX_TELEMETRY
|
#undef USE_SPEKTRUM_VTX_TELEMETRY
|
||||||
|
|
|
@ -76,7 +76,7 @@
|
||||||
|
|
||||||
#define USE_MAG
|
#define USE_MAG
|
||||||
|
|
||||||
#if !defined(USE_BARO) && !defined(USE_FAKE_BARO)
|
#if !defined(USE_BARO) && !defined(USE_VIRTUAL_BARO)
|
||||||
#define USE_BARO
|
#define USE_BARO
|
||||||
|
|
||||||
#define USE_BARO_MS5611
|
#define USE_BARO_MS5611
|
||||||
|
@ -319,7 +319,7 @@
|
||||||
#define USE_SPEKTRUM_BIND
|
#define USE_SPEKTRUM_BIND
|
||||||
#define USE_SPEKTRUM_BIND_PLUG
|
#define USE_SPEKTRUM_BIND_PLUG
|
||||||
#define USE_SPEKTRUM_REAL_RSSI
|
#define USE_SPEKTRUM_REAL_RSSI
|
||||||
#define USE_SPEKTRUM_FAKE_RSSI
|
#define USE_SPEKTRUM_VIRTUAL_RSSI
|
||||||
#define USE_SPEKTRUM_RSSI_PERCENT_CONVERSION
|
#define USE_SPEKTRUM_RSSI_PERCENT_CONVERSION
|
||||||
#define USE_SPEKTRUM_VTX_CONTROL
|
#define USE_SPEKTRUM_VTX_CONTROL
|
||||||
#define USE_SPEKTRUM_VTX_TELEMETRY
|
#define USE_SPEKTRUM_VTX_TELEMETRY
|
||||||
|
|
|
@ -301,7 +301,7 @@ sensor_gyro_unittest_SRC := \
|
||||||
$(USER_DIR)/common/maths.c \
|
$(USER_DIR)/common/maths.c \
|
||||||
$(USER_DIR)/common/streambuf.c \
|
$(USER_DIR)/common/streambuf.c \
|
||||||
$(USER_DIR)/common/sensor_alignment.c \
|
$(USER_DIR)/common/sensor_alignment.c \
|
||||||
$(USER_DIR)/drivers/accgyro/accgyro_fake.c \
|
$(USER_DIR)/drivers/accgyro/accgyro_virtual.c \
|
||||||
$(USER_DIR)/drivers/accgyro/gyro_sync.c \
|
$(USER_DIR)/drivers/accgyro/gyro_sync.c \
|
||||||
$(USER_DIR)/pg/pg.c \
|
$(USER_DIR)/pg/pg.c \
|
||||||
$(USER_DIR)/pg/gyrodev.c
|
$(USER_DIR)/pg/gyrodev.c
|
||||||
|
|
|
@ -29,7 +29,7 @@ extern "C" {
|
||||||
#include "common/axis.h"
|
#include "common/axis.h"
|
||||||
#include "common/maths.h"
|
#include "common/maths.h"
|
||||||
#include "common/utils.h"
|
#include "common/utils.h"
|
||||||
#include "drivers/accgyro/accgyro_fake.h"
|
#include "drivers/accgyro/accgyro_virtual.h"
|
||||||
#include "drivers/accgyro/accgyro_mpu.h"
|
#include "drivers/accgyro/accgyro_mpu.h"
|
||||||
#include "drivers/sensor.h"
|
#include "drivers/sensor.h"
|
||||||
#include "io/beeper.h"
|
#include "io/beeper.h"
|
||||||
|
@ -44,7 +44,7 @@ extern "C" {
|
||||||
STATIC_UNIT_TESTED gyroHardware_e gyroDetect(gyroDev_t *dev);
|
STATIC_UNIT_TESTED gyroHardware_e gyroDetect(gyroDev_t *dev);
|
||||||
struct gyroSensor_s;
|
struct gyroSensor_s;
|
||||||
STATIC_UNIT_TESTED void performGyroCalibration(struct gyroSensor_s *gyroSensor, uint8_t gyroMovementCalibrationThreshold);
|
STATIC_UNIT_TESTED void performGyroCalibration(struct gyroSensor_s *gyroSensor, uint8_t gyroMovementCalibrationThreshold);
|
||||||
STATIC_UNIT_TESTED bool fakeGyroRead(gyroDev_t *gyro);
|
STATIC_UNIT_TESTED bool virtualGyroRead(gyroDev_t *gyro);
|
||||||
|
|
||||||
uint8_t debugMode;
|
uint8_t debugMode;
|
||||||
int16_t debug[DEBUG16_VALUE_COUNT];
|
int16_t debug[DEBUG16_VALUE_COUNT];
|
||||||
|
@ -59,7 +59,7 @@ extern gyroDev_t * const gyroDevPtr;
|
||||||
TEST(SensorGyro, Detect)
|
TEST(SensorGyro, Detect)
|
||||||
{
|
{
|
||||||
const gyroHardware_e detected = gyroDetect(gyroDevPtr);
|
const gyroHardware_e detected = gyroDetect(gyroDevPtr);
|
||||||
EXPECT_EQ(GYRO_FAKE, detected);
|
EXPECT_EQ(GYRO_VIRTUAL, detected);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST(SensorGyro, Init)
|
TEST(SensorGyro, Init)
|
||||||
|
@ -67,14 +67,14 @@ TEST(SensorGyro, Init)
|
||||||
pgResetAll();
|
pgResetAll();
|
||||||
const bool initialised = gyroInit();
|
const bool initialised = gyroInit();
|
||||||
EXPECT_TRUE(initialised);
|
EXPECT_TRUE(initialised);
|
||||||
EXPECT_EQ(GYRO_FAKE, detectedSensors[SENSOR_INDEX_GYRO]);
|
EXPECT_EQ(GYRO_VIRTUAL, detectedSensors[SENSOR_INDEX_GYRO]);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST(SensorGyro, Read)
|
TEST(SensorGyro, Read)
|
||||||
{
|
{
|
||||||
pgResetAll();
|
pgResetAll();
|
||||||
gyroInit();
|
gyroInit();
|
||||||
fakeGyroSet(gyroDevPtr, 5, 6, 7);
|
virtualGyroSet(gyroDevPtr, 5, 6, 7);
|
||||||
const bool read = gyroDevPtr->readFn(gyroDevPtr);
|
const bool read = gyroDevPtr->readFn(gyroDevPtr);
|
||||||
EXPECT_TRUE(read);
|
EXPECT_TRUE(read);
|
||||||
EXPECT_EQ(5, gyroDevPtr->gyroADCRaw[X]);
|
EXPECT_EQ(5, gyroDevPtr->gyroADCRaw[X]);
|
||||||
|
@ -87,7 +87,7 @@ TEST(SensorGyro, Calibrate)
|
||||||
pgResetAll();
|
pgResetAll();
|
||||||
gyroInit();
|
gyroInit();
|
||||||
gyroSetTargetLooptime(1);
|
gyroSetTargetLooptime(1);
|
||||||
fakeGyroSet(gyroDevPtr, 5, 6, 7);
|
virtualGyroSet(gyroDevPtr, 5, 6, 7);
|
||||||
const bool read = gyroDevPtr->readFn(gyroDevPtr);
|
const bool read = gyroDevPtr->readFn(gyroDevPtr);
|
||||||
EXPECT_TRUE(read);
|
EXPECT_TRUE(read);
|
||||||
EXPECT_EQ(5, gyroDevPtr->gyroADCRaw[X]);
|
EXPECT_EQ(5, gyroDevPtr->gyroADCRaw[X]);
|
||||||
|
@ -122,14 +122,14 @@ TEST(SensorGyro, Update)
|
||||||
gyroConfigMutable()->gyro_soft_notch_hz_2 = 0;
|
gyroConfigMutable()->gyro_soft_notch_hz_2 = 0;
|
||||||
gyroInit();
|
gyroInit();
|
||||||
gyroSetTargetLooptime(1);
|
gyroSetTargetLooptime(1);
|
||||||
gyroDevPtr->readFn = fakeGyroRead;
|
gyroDevPtr->readFn = virtualGyroRead;
|
||||||
gyroStartCalibration(false);
|
gyroStartCalibration(false);
|
||||||
EXPECT_FALSE(gyroIsCalibrationComplete());
|
EXPECT_FALSE(gyroIsCalibrationComplete());
|
||||||
|
|
||||||
fakeGyroSet(gyroDevPtr, 5, 6, 7);
|
virtualGyroSet(gyroDevPtr, 5, 6, 7);
|
||||||
gyroUpdate();
|
gyroUpdate();
|
||||||
while (!gyroIsCalibrationComplete()) {
|
while (!gyroIsCalibrationComplete()) {
|
||||||
fakeGyroSet(gyroDevPtr, 5, 6, 7);
|
virtualGyroSet(gyroDevPtr, 5, 6, 7);
|
||||||
gyroUpdate();
|
gyroUpdate();
|
||||||
}
|
}
|
||||||
EXPECT_TRUE(gyroIsCalibrationComplete());
|
EXPECT_TRUE(gyroIsCalibrationComplete());
|
||||||
|
@ -144,7 +144,7 @@ TEST(SensorGyro, Update)
|
||||||
EXPECT_FLOAT_EQ(0, gyro.gyroADCf[X]);
|
EXPECT_FLOAT_EQ(0, gyro.gyroADCf[X]);
|
||||||
EXPECT_FLOAT_EQ(0, gyro.gyroADCf[Y]);
|
EXPECT_FLOAT_EQ(0, gyro.gyroADCf[Y]);
|
||||||
EXPECT_FLOAT_EQ(0, gyro.gyroADCf[Z]);
|
EXPECT_FLOAT_EQ(0, gyro.gyroADCf[Z]);
|
||||||
fakeGyroSet(gyroDevPtr, 15, 26, 97);
|
virtualGyroSet(gyroDevPtr, 15, 26, 97);
|
||||||
gyroUpdate();
|
gyroUpdate();
|
||||||
EXPECT_NEAR(10 * gyroDevPtr->scale, gyro.gyroADC[X], 1e-3); // gyro.gyroADC values are scaled
|
EXPECT_NEAR(10 * gyroDevPtr->scale, gyro.gyroADC[X], 1e-3); // gyro.gyroADC values are scaled
|
||||||
EXPECT_NEAR(20 * gyroDevPtr->scale, gyro.gyroADC[Y], 1e-3);
|
EXPECT_NEAR(20 * gyroDevPtr->scale, gyro.gyroADC[Y], 1e-3);
|
||||||
|
|
|
@ -26,7 +26,7 @@
|
||||||
#define USE_ACC
|
#define USE_ACC
|
||||||
#define USE_CMS
|
#define USE_CMS
|
||||||
#define CMS_MAX_DEVICE 4
|
#define CMS_MAX_DEVICE 4
|
||||||
#define USE_FAKE_GYRO
|
#define USE_VIRTUAL_GYRO
|
||||||
#define USE_BEEPER
|
#define USE_BEEPER
|
||||||
#define USE_BLACKBOX
|
#define USE_BLACKBOX
|
||||||
#define USE_MAG
|
#define USE_MAG
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue