Split gyro device out of gyro sensor struct
This commit is contained in:
parent
9a8124ffc4
commit
f2b423525d
|
@ -20,6 +20,8 @@
|
||||||
|
|
||||||
#include "platform.h"
|
#include "platform.h"
|
||||||
|
|
||||||
|
#ifdef USE_FAKE_GYRO
|
||||||
|
|
||||||
#include "common/axis.h"
|
#include "common/axis.h"
|
||||||
#include "common/utils.h"
|
#include "common/utils.h"
|
||||||
|
|
||||||
|
@ -27,8 +29,6 @@
|
||||||
#include "accgyro_fake.h"
|
#include "accgyro_fake.h"
|
||||||
|
|
||||||
|
|
||||||
#ifdef USE_FAKE_GYRO
|
|
||||||
|
|
||||||
static int16_t fakeGyroADC[XYZ_AXIS_COUNT];
|
static int16_t fakeGyroADC[XYZ_AXIS_COUNT];
|
||||||
|
|
||||||
static void fakeGyroInit(gyroDev_t *gyro)
|
static void fakeGyroInit(gyroDev_t *gyro)
|
||||||
|
|
|
@ -93,7 +93,6 @@ int16_t headFreeModeHold;
|
||||||
|
|
||||||
uint8_t motorControlEnable = false;
|
uint8_t motorControlEnable = false;
|
||||||
|
|
||||||
int16_t telemTemperature1; // gyro sensor temperature
|
|
||||||
static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero
|
static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero
|
||||||
|
|
||||||
bool isRXDataNew;
|
bool isRXDataNew;
|
||||||
|
@ -470,8 +469,8 @@ static void subTaskMainSubprocesses(timeUs_t currentTimeUs)
|
||||||
if (debugMode == DEBUG_PIDLOOP) {startTime = micros();}
|
if (debugMode == DEBUG_PIDLOOP) {startTime = micros();}
|
||||||
|
|
||||||
// Read out gyro temperature if used for telemmetry
|
// Read out gyro temperature if used for telemmetry
|
||||||
if (feature(FEATURE_TELEMETRY) && gyro.dev.temperature) {
|
if (feature(FEATURE_TELEMETRY)) {
|
||||||
gyro.dev.temperature(&gyro.dev, &telemTemperature1);
|
gyroReadTemperature();
|
||||||
}
|
}
|
||||||
|
|
||||||
#ifdef MAG
|
#ifdef MAG
|
||||||
|
|
|
@ -664,7 +664,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
|
||||||
sbufWriteU16(dst, acc.accSmooth[i] / scale);
|
sbufWriteU16(dst, acc.accSmooth[i] / scale);
|
||||||
}
|
}
|
||||||
for (int i = 0; i < 3; i++) {
|
for (int i = 0; i < 3; i++) {
|
||||||
sbufWriteU16(dst, lrintf(gyro.gyroADCf[i] / gyro.dev.scale));
|
sbufWriteU16(dst, gyroRateDps(i));
|
||||||
}
|
}
|
||||||
for (int i = 0; i < 3; i++) {
|
for (int i = 0; i < 3; i++) {
|
||||||
sbufWriteU16(dst, mag.magADC[i]);
|
sbufWriteU16(dst, mag.magADC[i]);
|
||||||
|
|
|
@ -35,18 +35,18 @@
|
||||||
#include "drivers/accgyro_bma280.h"
|
#include "drivers/accgyro_bma280.h"
|
||||||
#include "drivers/accgyro_fake.h"
|
#include "drivers/accgyro_fake.h"
|
||||||
#include "drivers/accgyro_l3g4200d.h"
|
#include "drivers/accgyro_l3g4200d.h"
|
||||||
|
#include "drivers/accgyro_l3gd20.h"
|
||||||
|
#include "drivers/accgyro_lsm303dlhc.h"
|
||||||
#include "drivers/accgyro_mma845x.h"
|
#include "drivers/accgyro_mma845x.h"
|
||||||
#include "drivers/accgyro_mpu.h"
|
#include "drivers/accgyro_mpu.h"
|
||||||
#include "drivers/accgyro_mpu3050.h"
|
#include "drivers/accgyro_mpu3050.h"
|
||||||
#include "drivers/accgyro_mpu6050.h"
|
#include "drivers/accgyro_mpu6050.h"
|
||||||
#include "drivers/accgyro_mpu6500.h"
|
#include "drivers/accgyro_mpu6500.h"
|
||||||
#include "drivers/accgyro_l3gd20.h"
|
|
||||||
#include "drivers/accgyro_lsm303dlhc.h"
|
|
||||||
#include "drivers/bus_spi.h"
|
|
||||||
#include "drivers/accgyro_spi_icm20689.h"
|
#include "drivers/accgyro_spi_icm20689.h"
|
||||||
#include "drivers/accgyro_spi_mpu6000.h"
|
#include "drivers/accgyro_spi_mpu6000.h"
|
||||||
#include "drivers/accgyro_spi_mpu6500.h"
|
#include "drivers/accgyro_spi_mpu6500.h"
|
||||||
#include "drivers/accgyro_spi_mpu9250.h"
|
#include "drivers/accgyro_spi_mpu9250.h"
|
||||||
|
#include "drivers/bus_spi.h"
|
||||||
#include "drivers/system.h"
|
#include "drivers/system.h"
|
||||||
|
|
||||||
#include "fc/config.h"
|
#include "fc/config.h"
|
||||||
|
@ -250,8 +250,8 @@ bool accInit(uint32_t gyroSamplingInverval)
|
||||||
{
|
{
|
||||||
memset(&acc, 0, sizeof(acc));
|
memset(&acc, 0, sizeof(acc));
|
||||||
// copy over the common gyro mpu settings
|
// copy over the common gyro mpu settings
|
||||||
acc.dev.mpuConfiguration = gyro.dev.mpuConfiguration;
|
acc.dev.mpuConfiguration = *gyroMpuConfiguration();
|
||||||
acc.dev.mpuDetectionResult = gyro.dev.mpuDetectionResult;
|
acc.dev.mpuDetectionResult = *gyroMpuDetectionResult();
|
||||||
if (!accDetect(&acc.dev, accelerometerConfig()->acc_hardware)) {
|
if (!accDetect(&acc.dev, accelerometerConfig()->acc_hardware)) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
@ -278,6 +278,9 @@ bool accInit(uint32_t gyroSamplingInverval)
|
||||||
biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, acc.accSamplingInterval);
|
biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, acc.accSamplingInterval);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
if (accelerometerConfig()->acc_align != ALIGN_DEFAULT) {
|
||||||
|
acc.dev.accAlign = accelerometerConfig()->acc_align;
|
||||||
|
}
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -44,6 +44,7 @@
|
||||||
#include "hardware_revision.h"
|
#include "hardware_revision.h"
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
magDev_t magDev;
|
||||||
mag_t mag; // mag access functions
|
mag_t mag; // mag access functions
|
||||||
|
|
||||||
#ifdef MAG
|
#ifdef MAG
|
||||||
|
@ -146,17 +147,25 @@ retry:
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
void compassInit(void)
|
bool compassInit(void)
|
||||||
{
|
{
|
||||||
// initialize and calibration. turn on led during mag calibration (calibration routine blinks it)
|
// initialize and calibration. turn on led during mag calibration (calibration routine blinks it)
|
||||||
// calculate magnetic declination
|
// calculate magnetic declination
|
||||||
|
mag.magneticDeclination = 0.0f; // TODO investigate if this is actually needed if there is no mag sensor or if the value stored in the config should be used.
|
||||||
|
if (!compassDetect(&magDev, compassConfig()->mag_hardware)) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
const int16_t deg = compassConfig()->mag_declination / 100;
|
const int16_t deg = compassConfig()->mag_declination / 100;
|
||||||
const int16_t min = compassConfig()->mag_declination % 100;
|
const int16_t min = compassConfig()->mag_declination % 100;
|
||||||
mag.magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units
|
mag.magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units
|
||||||
LED1_ON;
|
LED1_ON;
|
||||||
mag.dev.init();
|
magDev.init();
|
||||||
LED1_OFF;
|
LED1_OFF;
|
||||||
magInit = 1;
|
magInit = 1;
|
||||||
|
if (compassConfig()->mag_align != ALIGN_DEFAULT) {
|
||||||
|
magDev.magAlign = compassConfig()->mag_align;
|
||||||
|
}
|
||||||
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
void compassUpdate(uint32_t currentTime, flightDynamicsTrims_t *magZero)
|
void compassUpdate(uint32_t currentTime, flightDynamicsTrims_t *magZero)
|
||||||
|
@ -165,11 +174,11 @@ void compassUpdate(uint32_t currentTime, flightDynamicsTrims_t *magZero)
|
||||||
static flightDynamicsTrims_t magZeroTempMin;
|
static flightDynamicsTrims_t magZeroTempMin;
|
||||||
static flightDynamicsTrims_t magZeroTempMax;
|
static flightDynamicsTrims_t magZeroTempMax;
|
||||||
|
|
||||||
mag.dev.read(magADCRaw);
|
magDev.read(magADCRaw);
|
||||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||||
mag.magADC[axis] = magADCRaw[axis];
|
mag.magADC[axis] = magADCRaw[axis];
|
||||||
}
|
}
|
||||||
alignSensors(mag.magADC, mag.dev.magAlign);
|
alignSensors(mag.magADC, magDev.magAlign);
|
||||||
|
|
||||||
if (STATE(CALIBRATE_MAG)) {
|
if (STATE(CALIBRATE_MAG)) {
|
||||||
tCal = currentTime;
|
tCal = currentTime;
|
||||||
|
|
|
@ -18,7 +18,7 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include "config/parameter_group.h"
|
#include "config/parameter_group.h"
|
||||||
#include "drivers/compass.h"
|
#include "drivers/sensor.h"
|
||||||
#include "sensors/sensors.h"
|
#include "sensors/sensors.h"
|
||||||
|
|
||||||
|
|
||||||
|
@ -32,7 +32,6 @@ typedef enum {
|
||||||
} magSensor_e;
|
} magSensor_e;
|
||||||
|
|
||||||
typedef struct mag_s {
|
typedef struct mag_s {
|
||||||
magDev_t dev;
|
|
||||||
int32_t magADC[XYZ_AXIS_COUNT];
|
int32_t magADC[XYZ_AXIS_COUNT];
|
||||||
float magneticDeclination;
|
float magneticDeclination;
|
||||||
} mag_t;
|
} mag_t;
|
||||||
|
@ -49,8 +48,7 @@ typedef struct compassConfig_s {
|
||||||
|
|
||||||
PG_DECLARE(compassConfig_t, compassConfig);
|
PG_DECLARE(compassConfig_t, compassConfig);
|
||||||
|
|
||||||
bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse);
|
bool compassInit(void);
|
||||||
void compassInit(void);
|
|
||||||
union flightDynamicsTrims_u;
|
union flightDynamicsTrims_u;
|
||||||
void compassUpdate(uint32_t currentTime, union flightDynamicsTrims_u *magZero);
|
void compassUpdate(uint32_t currentTime, union flightDynamicsTrims_u *magZero);
|
||||||
|
|
||||||
|
|
|
@ -36,13 +36,13 @@
|
||||||
#include "drivers/accgyro_bma280.h"
|
#include "drivers/accgyro_bma280.h"
|
||||||
#include "drivers/accgyro_fake.h"
|
#include "drivers/accgyro_fake.h"
|
||||||
#include "drivers/accgyro_l3g4200d.h"
|
#include "drivers/accgyro_l3g4200d.h"
|
||||||
|
#include "drivers/accgyro_l3gd20.h"
|
||||||
|
#include "drivers/accgyro_lsm303dlhc.h"
|
||||||
#include "drivers/accgyro_mma845x.h"
|
#include "drivers/accgyro_mma845x.h"
|
||||||
#include "drivers/accgyro_mpu.h"
|
#include "drivers/accgyro_mpu.h"
|
||||||
#include "drivers/accgyro_mpu3050.h"
|
#include "drivers/accgyro_mpu3050.h"
|
||||||
#include "drivers/accgyro_mpu6050.h"
|
#include "drivers/accgyro_mpu6050.h"
|
||||||
#include "drivers/accgyro_mpu6500.h"
|
#include "drivers/accgyro_mpu6500.h"
|
||||||
#include "drivers/accgyro_l3gd20.h"
|
|
||||||
#include "drivers/accgyro_lsm303dlhc.h"
|
|
||||||
#include "drivers/accgyro_spi_icm20689.h"
|
#include "drivers/accgyro_spi_icm20689.h"
|
||||||
#include "drivers/accgyro_spi_mpu6000.h"
|
#include "drivers/accgyro_spi_mpu6000.h"
|
||||||
#include "drivers/accgyro_spi_mpu6500.h"
|
#include "drivers/accgyro_spi_mpu6500.h"
|
||||||
|
@ -59,19 +59,22 @@
|
||||||
|
|
||||||
#include "scheduler/scheduler.h"
|
#include "scheduler/scheduler.h"
|
||||||
|
|
||||||
#include "sensors/sensors.h"
|
|
||||||
#include "sensors/boardalignment.h"
|
#include "sensors/boardalignment.h"
|
||||||
#include "sensors/gyro.h"
|
#include "sensors/gyro.h"
|
||||||
|
#include "sensors/sensors.h"
|
||||||
|
|
||||||
#ifdef USE_HARDWARE_REVISION_DETECTION
|
#ifdef USE_HARDWARE_REVISION_DETECTION
|
||||||
#include "hardware_revision.h"
|
#include "hardware_revision.h"
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
gyro_t gyro; // gyro access functions
|
gyro_t gyro;
|
||||||
|
|
||||||
|
STATIC_UNIT_TESTED gyroDev_t gyroDev0;
|
||||||
|
static int16_t gyroTemperature0;
|
||||||
|
|
||||||
static int32_t gyroADC[XYZ_AXIS_COUNT];
|
static int32_t gyroADC[XYZ_AXIS_COUNT];
|
||||||
|
|
||||||
static int32_t gyroZero[XYZ_AXIS_COUNT] = { 0, 0, 0 };
|
STATIC_UNIT_TESTED int32_t gyroZero[XYZ_AXIS_COUNT] = { 0, 0, 0 };
|
||||||
static uint16_t calibratingG = 0;
|
static uint16_t calibratingG = 0;
|
||||||
|
|
||||||
static filterApplyFnPtr softLpfFilterApplyFn;
|
static filterApplyFnPtr softLpfFilterApplyFn;
|
||||||
|
@ -106,6 +109,7 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
|
||||||
.gyroMovementCalibrationThreshold = 32
|
.gyroMovementCalibrationThreshold = 32
|
||||||
);
|
);
|
||||||
|
|
||||||
|
#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689)
|
||||||
static const extiConfig_t *selectMPUIntExtiConfig(void)
|
static const extiConfig_t *selectMPUIntExtiConfig(void)
|
||||||
{
|
{
|
||||||
#if defined(MPU_INT_EXTI)
|
#if defined(MPU_INT_EXTI)
|
||||||
|
@ -117,8 +121,18 @@ static const extiConfig_t *selectMPUIntExtiConfig(void)
|
||||||
return NULL;
|
return NULL;
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
static bool gyroDetect(gyroDev_t *dev)
|
const mpuConfiguration_t *gyroMpuConfiguration(void)
|
||||||
|
{
|
||||||
|
return &gyroDev0.mpuConfiguration;
|
||||||
|
}
|
||||||
|
const mpuDetectionResult_t *gyroMpuDetectionResult(void)
|
||||||
|
{
|
||||||
|
return &gyroDev0.mpuDetectionResult;
|
||||||
|
}
|
||||||
|
|
||||||
|
STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev)
|
||||||
{
|
{
|
||||||
gyroSensor_e gyroHardware = GYRO_DEFAULT;
|
gyroSensor_e gyroHardware = GYRO_DEFAULT;
|
||||||
|
|
||||||
|
@ -246,34 +260,29 @@ static bool gyroDetect(gyroDev_t *dev)
|
||||||
gyroHardware = GYRO_NONE;
|
gyroHardware = GYRO_NONE;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (gyroHardware == GYRO_NONE) {
|
if (gyroHardware != GYRO_NONE) {
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware;
|
detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware;
|
||||||
sensorsSet(SENSOR_GYRO);
|
sensorsSet(SENSOR_GYRO);
|
||||||
|
}
|
||||||
|
|
||||||
return true;
|
|
||||||
|
return gyroHardware;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool gyroInit(void)
|
bool gyroInit(void)
|
||||||
{
|
{
|
||||||
memset(&gyro, 0, sizeof(gyro));
|
memset(&gyro, 0, sizeof(gyro));
|
||||||
#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689)
|
#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689)
|
||||||
gyro.dev.mpuIntExtiConfig = selectMPUIntExtiConfig();
|
gyroDev0.mpuIntExtiConfig = selectMPUIntExtiConfig();
|
||||||
mpuDetect(&gyro.dev);
|
mpuDetect(&gyroDev0);
|
||||||
mpuResetFn = gyro.dev.mpuConfiguration.resetFn;
|
mpuResetFn = gyroDev0.mpuConfiguration.resetFn;
|
||||||
#endif
|
#endif
|
||||||
|
const gyroSensor_e gyroHardware = gyroDetect(&gyroDev0);
|
||||||
if (!gyroDetect(&gyro.dev)) {
|
if (gyroHardware == GYRO_NONE) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
switch (detectedSensors[SENSOR_INDEX_GYRO]) {
|
switch (gyroHardware) {
|
||||||
default:
|
|
||||||
// gyro does not support 32kHz
|
|
||||||
gyroConfigMutable()->gyro_use_32khz = false;
|
|
||||||
break;
|
|
||||||
case GYRO_MPU6500:
|
case GYRO_MPU6500:
|
||||||
case GYRO_MPU9250:
|
case GYRO_MPU9250:
|
||||||
case GYRO_ICM20689:
|
case GYRO_ICM20689:
|
||||||
|
@ -281,12 +290,19 @@ bool gyroInit(void)
|
||||||
case GYRO_ICM20602:
|
case GYRO_ICM20602:
|
||||||
// do nothing, as gyro supports 32kHz
|
// do nothing, as gyro supports 32kHz
|
||||||
break;
|
break;
|
||||||
|
default:
|
||||||
|
// gyro does not support 32kHz
|
||||||
|
gyroConfigMutable()->gyro_use_32khz = false;
|
||||||
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Must set gyro sample rate before initialisation
|
// Must set gyro sample rate before initialisation
|
||||||
gyro.targetLooptime = gyroSetSampleRate(&gyro.dev, gyroConfig()->gyro_lpf, gyroConfig()->gyro_sync_denom, gyroConfig()->gyro_use_32khz);
|
gyro.targetLooptime = gyroSetSampleRate(&gyroDev0, gyroConfig()->gyro_lpf, gyroConfig()->gyro_sync_denom, gyroConfig()->gyro_use_32khz);
|
||||||
gyro.dev.lpf = gyroConfig()->gyro_lpf;
|
gyroDev0.lpf = gyroConfig()->gyro_lpf;
|
||||||
gyro.dev.init(&gyro.dev);
|
gyroDev0.init(&gyroDev0);
|
||||||
|
if (gyroConfig()->gyro_align != ALIGN_DEFAULT) {
|
||||||
|
gyroDev0.gyroAlign = gyroConfig()->gyro_align;
|
||||||
|
}
|
||||||
gyroInitFilters();
|
gyroInitFilters();
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
@ -369,7 +385,7 @@ void gyroSetCalibrationCycles(void)
|
||||||
calibratingG = gyroCalculateCalibratingCycles();
|
calibratingG = gyroCalculateCalibratingCycles();
|
||||||
}
|
}
|
||||||
|
|
||||||
static void performGyroCalibration(uint8_t gyroMovementCalibrationThreshold)
|
STATIC_UNIT_TESTED void performGyroCalibration(uint8_t gyroMovementCalibrationThreshold)
|
||||||
{
|
{
|
||||||
static int32_t g[3];
|
static int32_t g[3];
|
||||||
static stdev_t var[3];
|
static stdev_t var[3];
|
||||||
|
@ -445,27 +461,27 @@ static bool gyroUpdateISR(gyroDev_t* gyroDev)
|
||||||
void gyroUpdate(void)
|
void gyroUpdate(void)
|
||||||
{
|
{
|
||||||
// range: +/- 8192; +/- 2000 deg/sec
|
// range: +/- 8192; +/- 2000 deg/sec
|
||||||
if (gyro.dev.update) {
|
if (gyroDev0.update) {
|
||||||
// if the gyro update function is set then return, since the gyro is read in gyroUpdateISR
|
// if the gyro update function is set then return, since the gyro is read in gyroUpdateISR
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
if (!gyro.dev.read(&gyro.dev)) {
|
if (!gyroDev0.read(&gyroDev0)) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
gyro.dev.dataReady = false;
|
gyroDev0.dataReady = false;
|
||||||
// move gyro data into 32-bit variables to avoid overflows in calculations
|
// move gyro data into 32-bit variables to avoid overflows in calculations
|
||||||
gyroADC[X] = gyro.dev.gyroADCRaw[X];
|
gyroADC[X] = gyroDev0.gyroADCRaw[X];
|
||||||
gyroADC[Y] = gyro.dev.gyroADCRaw[Y];
|
gyroADC[Y] = gyroDev0.gyroADCRaw[Y];
|
||||||
gyroADC[Z] = gyro.dev.gyroADCRaw[Z];
|
gyroADC[Z] = gyroDev0.gyroADCRaw[Z];
|
||||||
|
|
||||||
alignSensors(gyroADC, gyro.dev.gyroAlign);
|
alignSensors(gyroADC, gyroDev0.gyroAlign);
|
||||||
|
|
||||||
const bool calibrationComplete = isGyroCalibrationComplete();
|
const bool calibrationComplete = isGyroCalibrationComplete();
|
||||||
if (calibrationComplete) {
|
if (calibrationComplete) {
|
||||||
#if defined(GYRO_USES_SPI) && defined(USE_MPU_DATA_READY_SIGNAL)
|
#if defined(GYRO_USES_SPI) && defined(USE_MPU_DATA_READY_SIGNAL)
|
||||||
// SPI-based gyro so can read and update in ISR
|
// SPI-based gyro so can read and update in ISR
|
||||||
if (gyroConfig()->gyro_isr_update) {
|
if (gyroConfig()->gyro_isr_update) {
|
||||||
mpuGyroSetIsrUpdate(&gyro.dev, gyroUpdateISR);
|
mpuGyroSetIsrUpdate(&gyroDev0, gyroUpdateISR);
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
@ -479,7 +495,7 @@ void gyroUpdate(void)
|
||||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||||
gyroADC[axis] -= gyroZero[axis];
|
gyroADC[axis] -= gyroZero[axis];
|
||||||
// scale gyro output to degrees per second
|
// scale gyro output to degrees per second
|
||||||
float gyroADCf = (float)gyroADC[axis] * gyro.dev.scale;
|
float gyroADCf = (float)gyroADC[axis] * gyroDev0.scale;
|
||||||
|
|
||||||
// Apply LPF
|
// Apply LPF
|
||||||
DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf));
|
DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf));
|
||||||
|
@ -493,8 +509,25 @@ void gyroUpdate(void)
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!calibrationComplete) {
|
if (!calibrationComplete) {
|
||||||
gyroADC[X] = lrintf(gyro.gyroADCf[X] / gyro.dev.scale);
|
gyroADC[X] = lrintf(gyro.gyroADCf[X] / gyroDev0.scale);
|
||||||
gyroADC[Y] = lrintf(gyro.gyroADCf[Y] / gyro.dev.scale);
|
gyroADC[Y] = lrintf(gyro.gyroADCf[Y] / gyroDev0.scale);
|
||||||
gyroADC[Z] = lrintf(gyro.gyroADCf[Z] / gyro.dev.scale);
|
gyroADC[Z] = lrintf(gyro.gyroADCf[Z] / gyroDev0.scale);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void gyroReadTemperature(void)
|
||||||
|
{
|
||||||
|
if (gyroDev0.temperature) {
|
||||||
|
gyroDev0.temperature(&gyroDev0, &gyroTemperature0);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
int16_t gyroGetTemperature(void)
|
||||||
|
{
|
||||||
|
return gyroTemperature0;
|
||||||
|
}
|
||||||
|
|
||||||
|
int16_t gyroRateDps(int axis)
|
||||||
|
{
|
||||||
|
return lrintf(gyro.gyroADCf[axis] / gyroDev0.scale);
|
||||||
|
}
|
||||||
|
|
|
@ -18,7 +18,6 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include "config/parameter_group.h"
|
#include "config/parameter_group.h"
|
||||||
#include "drivers/accgyro.h"
|
|
||||||
#include "common/axis.h"
|
#include "common/axis.h"
|
||||||
|
|
||||||
typedef enum {
|
typedef enum {
|
||||||
|
@ -38,7 +37,6 @@ typedef enum {
|
||||||
} gyroSensor_e;
|
} gyroSensor_e;
|
||||||
|
|
||||||
typedef struct gyro_s {
|
typedef struct gyro_s {
|
||||||
gyroDev_t dev;
|
|
||||||
uint32_t targetLooptime;
|
uint32_t targetLooptime;
|
||||||
float gyroADCf[XYZ_AXIS_COUNT];
|
float gyroADCf[XYZ_AXIS_COUNT];
|
||||||
} gyro_t;
|
} gyro_t;
|
||||||
|
@ -62,8 +60,13 @@ typedef struct gyroConfig_s {
|
||||||
|
|
||||||
PG_DECLARE(gyroConfig_t, gyroConfig);
|
PG_DECLARE(gyroConfig_t, gyroConfig);
|
||||||
|
|
||||||
void gyroSetCalibrationCycles(void);
|
|
||||||
bool gyroInit(void);
|
bool gyroInit(void);
|
||||||
void gyroInitFilters(void);
|
void gyroInitFilters(void);
|
||||||
void gyroUpdate(void);
|
void gyroUpdate(void);
|
||||||
|
const mpuConfiguration_t *gyroMpuConfiguration(void);
|
||||||
|
const mpuDetectionResult_t *gyroMpuDetectionResult(void);
|
||||||
|
void gyroSetCalibrationCycles(void);
|
||||||
bool isGyroCalibrationComplete(void);
|
bool isGyroCalibrationComplete(void);
|
||||||
|
void gyroReadTemperature(void);
|
||||||
|
int16_t gyroGetTemperature(void);
|
||||||
|
int16_t gyroRateDps(int axis);
|
||||||
|
|
|
@ -63,11 +63,8 @@ bool sensorsAutodetect(void)
|
||||||
|
|
||||||
accInit(gyro.targetLooptime);
|
accInit(gyro.targetLooptime);
|
||||||
|
|
||||||
mag.magneticDeclination = 0.0f; // TODO investigate if this is actually needed if there is no mag sensor or if the value stored in the config should be used.
|
|
||||||
#ifdef MAG
|
#ifdef MAG
|
||||||
if (compassDetect(&mag.dev, compassConfig()->mag_hardware)) {
|
|
||||||
compassInit();
|
compassInit();
|
||||||
}
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef BARO
|
#ifdef BARO
|
||||||
|
@ -80,15 +77,5 @@ bool sensorsAutodetect(void)
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
if (gyroConfig()->gyro_align != ALIGN_DEFAULT) {
|
|
||||||
gyro.dev.gyroAlign = gyroConfig()->gyro_align;
|
|
||||||
}
|
|
||||||
if (accelerometerConfig()->acc_align != ALIGN_DEFAULT) {
|
|
||||||
acc.dev.accAlign = accelerometerConfig()->acc_align;
|
|
||||||
}
|
|
||||||
if (compassConfig()->mag_align != ALIGN_DEFAULT) {
|
|
||||||
mag.dev.magAlign = compassConfig()->mag_align;
|
|
||||||
}
|
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
|
@ -25,7 +25,6 @@ typedef enum {
|
||||||
SENSOR_INDEX_COUNT
|
SENSOR_INDEX_COUNT
|
||||||
} sensorIndex_e;
|
} sensorIndex_e;
|
||||||
|
|
||||||
extern int16_t telemTemperature1; //FIXME move to temp sensor...?
|
|
||||||
extern uint8_t detectedSensors[SENSOR_INDEX_COUNT];
|
extern uint8_t detectedSensors[SENSOR_INDEX_COUNT];
|
||||||
|
|
||||||
typedef struct int16_flightDynamicsTrims_s {
|
typedef struct int16_flightDynamicsTrims_s {
|
||||||
|
|
|
@ -613,7 +613,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
|
||||||
bstWrite16(acc.accSmooth[i] / scale);
|
bstWrite16(acc.accSmooth[i] / scale);
|
||||||
}
|
}
|
||||||
for (i = 0; i < 3; i++) {
|
for (i = 0; i < 3; i++) {
|
||||||
bstWrite16(lrintf(gyro.gyroADCf[i] /gyro.dev.scale));
|
bstWrite16(gyroRateDps(i));
|
||||||
}
|
}
|
||||||
for (i = 0; i < 3; i++) {
|
for (i = 0; i < 3; i++) {
|
||||||
bstWrite16(mag.magADC[i]);
|
bstWrite16(mag.magADC[i]);
|
||||||
|
|
|
@ -223,7 +223,7 @@ static void sendTemperature1(void)
|
||||||
#elif defined(BARO)
|
#elif defined(BARO)
|
||||||
serialize16((baro.baroTemperature + 50)/ 100); //Airmamaf
|
serialize16((baro.baroTemperature + 50)/ 100); //Airmamaf
|
||||||
#else
|
#else
|
||||||
serialize16(telemTemperature1 / 10);
|
serialize16(gyroGetTemperature() / 10);
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -300,7 +300,7 @@ static void dispatchMeasurementReply(ibusAddress_t address)
|
||||||
#ifdef BARO
|
#ifdef BARO
|
||||||
value = (baro.baroTemperature + 5) / 10; // +5 to make integer division rounding correct
|
value = (baro.baroTemperature + 5) / 10; // +5 to make integer division rounding correct
|
||||||
#else
|
#else
|
||||||
value = telemTemperature1 * 10;
|
value = gyroGetTemperature() * 10;
|
||||||
#endif
|
#endif
|
||||||
sendIbusMeasurement(address, value + IBUS_TEMPERATURE_OFFSET);
|
sendIbusMeasurement(address, value + IBUS_TEMPERATURE_OFFSET);
|
||||||
break;
|
break;
|
||||||
|
|
|
@ -675,6 +675,60 @@ $(OBJECT_DIR)/alignsensor_unittest : \
|
||||||
$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
|
$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
|
||||||
|
|
||||||
|
|
||||||
|
$(OBJECT_DIR)/build/debug.o : \
|
||||||
|
$(USER_DIR)/build/debug.c \
|
||||||
|
$(USER_DIR)/build/debug.h \
|
||||||
|
$(GTEST_HEADERS)
|
||||||
|
|
||||||
|
@mkdir -p $(dir $@)
|
||||||
|
$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/build/debug.c -o $@
|
||||||
|
|
||||||
|
$(OBJECT_DIR)/drivers/gyro_sync.o : \
|
||||||
|
$(USER_DIR)/drivers/gyro_sync.c \
|
||||||
|
$(USER_DIR)/drivers/gyro_sync.h \
|
||||||
|
$(GTEST_HEADERS)
|
||||||
|
|
||||||
|
@mkdir -p $(dir $@)
|
||||||
|
$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/drivers/gyro_sync.c -o $@
|
||||||
|
|
||||||
|
$(OBJECT_DIR)/drivers/accgyro_fake.o : \
|
||||||
|
$(USER_DIR)/drivers/accgyro_fake.c \
|
||||||
|
$(USER_DIR)/drivers/accgyro_fake.h \
|
||||||
|
$(GTEST_HEADERS)
|
||||||
|
|
||||||
|
@mkdir -p $(dir $@)
|
||||||
|
$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/drivers/accgyro_fake.c -o $@
|
||||||
|
|
||||||
|
$(OBJECT_DIR)/sensors/gyro.o : \
|
||||||
|
$(USER_DIR)/sensors/gyro.c \
|
||||||
|
$(USER_DIR)/sensors/gyro.h \
|
||||||
|
$(GTEST_HEADERS)
|
||||||
|
|
||||||
|
@mkdir -p $(dir $@)
|
||||||
|
$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/sensors/gyro.c -o $@
|
||||||
|
|
||||||
|
$(OBJECT_DIR)/sensor_gyro_unittest.o : \
|
||||||
|
$(TEST_DIR)/sensor_gyro_unittest.cc \
|
||||||
|
$(USER_DIR)/sensors/gyro.h \
|
||||||
|
$(GTEST_HEADERS)
|
||||||
|
|
||||||
|
@mkdir -p $(dir $@)
|
||||||
|
$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/sensor_gyro_unittest.cc -o $@
|
||||||
|
|
||||||
|
$(OBJECT_DIR)/sensor_gyro_unittest : \
|
||||||
|
$(OBJECT_DIR)/build/debug.o \
|
||||||
|
$(OBJECT_DIR)/common/filter.o \
|
||||||
|
$(OBJECT_DIR)/common/maths.o \
|
||||||
|
$(OBJECT_DIR)/drivers/accgyro_fake.o \
|
||||||
|
$(OBJECT_DIR)/drivers/gyro_sync.o \
|
||||||
|
$(OBJECT_DIR)/sensors/boardalignment.o \
|
||||||
|
$(OBJECT_DIR)/sensors/gyro.o \
|
||||||
|
$(OBJECT_DIR)/sensor_gyro_unittest.o \
|
||||||
|
$(OBJECT_DIR)/gtest_main.a
|
||||||
|
|
||||||
|
$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
|
||||||
|
|
||||||
|
|
||||||
$(OBJECT_DIR)/build/version.o : \
|
$(OBJECT_DIR)/build/version.o : \
|
||||||
$(USER_DIR)/build/version.c \
|
$(USER_DIR)/build/version.c \
|
||||||
$(USER_DIR)/build/version.h \
|
$(USER_DIR)/build/version.h \
|
||||||
|
|
|
@ -65,6 +65,11 @@ uint8_t DMA_GetFlagStatus(void *);
|
||||||
void DMA_Cmd(DMA_Channel_TypeDef*, FunctionalState );
|
void DMA_Cmd(DMA_Channel_TypeDef*, FunctionalState );
|
||||||
void DMA_ClearFlag(uint32_t);
|
void DMA_ClearFlag(uint32_t);
|
||||||
|
|
||||||
|
typedef struct
|
||||||
|
{
|
||||||
|
void* test;
|
||||||
|
} SPI_TypeDef;
|
||||||
|
|
||||||
typedef struct
|
typedef struct
|
||||||
{
|
{
|
||||||
void* test;
|
void* test;
|
||||||
|
|
|
@ -19,6 +19,7 @@
|
||||||
|
|
||||||
#define CMS
|
#define CMS
|
||||||
#define CMS_MAX_DEVICE 4
|
#define CMS_MAX_DEVICE 4
|
||||||
|
#define USE_FAKE_GYRO
|
||||||
#define MAG
|
#define MAG
|
||||||
#define BARO
|
#define BARO
|
||||||
#define GPS
|
#define GPS
|
||||||
|
|
Loading…
Reference in New Issue