mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-25 17:25:20 +03:00
Merge branch 'master' into patch_v3.1.6
This commit is contained in:
borisbstyle
GitHub
commit
de3d1d527d
197 changed files with 4476 additions and 4008 deletions
|
|
@ -28,18 +28,21 @@
|
|||
#include "common/maths.h"
|
||||
#include "common/filter.h"
|
||||
|
||||
#include "config/parameter_group.h"
|
||||
#include "config/parameter_group_ids.h"
|
||||
|
||||
#include "drivers/accgyro.h"
|
||||
#include "drivers/accgyro_adxl345.h"
|
||||
#include "drivers/accgyro_bma280.h"
|
||||
#include "drivers/accgyro_fake.h"
|
||||
#include "drivers/accgyro_l3g4200d.h"
|
||||
#include "drivers/accgyro_l3gd20.h"
|
||||
#include "drivers/accgyro_lsm303dlhc.h"
|
||||
#include "drivers/accgyro_mma845x.h"
|
||||
#include "drivers/accgyro_mpu.h"
|
||||
#include "drivers/accgyro_mpu3050.h"
|
||||
#include "drivers/accgyro_mpu6050.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_mpu6000.h"
|
||||
#include "drivers/accgyro_spi_mpu6500.h"
|
||||
|
|
@ -56,20 +59,22 @@
|
|||
|
||||
#include "scheduler/scheduler.h"
|
||||
|
||||
#include "sensors/sensors.h"
|
||||
#include "sensors/boardalignment.h"
|
||||
#include "sensors/gyro.h"
|
||||
#include "sensors/sensors.h"
|
||||
|
||||
#ifdef USE_HARDWARE_REVISION_DETECTION
|
||||
#include "hardware_revision.h"
|
||||
#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 gyroZero[XYZ_AXIS_COUNT] = { 0, 0, 0 };
|
||||
static const gyroConfig_t *gyroConfig;
|
||||
STATIC_UNIT_TESTED int32_t gyroZero[XYZ_AXIS_COUNT] = { 0, 0, 0 };
|
||||
static uint16_t calibratingG = 0;
|
||||
|
||||
static filterApplyFnPtr softLpfFilterApplyFn;
|
||||
|
|
@ -81,6 +86,30 @@ static void *notchFilter2[3];
|
|||
|
||||
#define DEBUG_GYRO_CALIBRATION 3
|
||||
|
||||
#ifdef STM32F10X
|
||||
#define GYRO_SYNC_DENOM_DEFAULT 8
|
||||
#elif defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_ICM20689)
|
||||
#define GYRO_SYNC_DENOM_DEFAULT 1
|
||||
#else
|
||||
#define GYRO_SYNC_DENOM_DEFAULT 4
|
||||
#endif
|
||||
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 0);
|
||||
|
||||
PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
|
||||
.gyro_lpf = GYRO_LPF_256HZ,
|
||||
.gyro_sync_denom = GYRO_SYNC_DENOM_DEFAULT,
|
||||
.gyro_soft_lpf_type = FILTER_PT1,
|
||||
.gyro_soft_lpf_hz = 90,
|
||||
.gyro_soft_notch_hz_1 = 400,
|
||||
.gyro_soft_notch_cutoff_1 = 300,
|
||||
.gyro_soft_notch_hz_2 = 200,
|
||||
.gyro_soft_notch_cutoff_2 = 100,
|
||||
.gyro_align = ALIGN_DEFAULT,
|
||||
.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)
|
||||
{
|
||||
#if defined(MPU_INT_EXTI)
|
||||
|
|
@ -92,8 +121,18 @@ static const extiConfig_t *selectMPUIntExtiConfig(void)
|
|||
return NULL;
|
||||
#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;
|
||||
|
||||
|
|
@ -221,36 +260,29 @@ static bool gyroDetect(gyroDev_t *dev)
|
|||
gyroHardware = GYRO_NONE;
|
||||
}
|
||||
|
||||
if (gyroHardware != GYRO_NONE) {
|
||||
detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware;
|
||||
sensorsSet(SENSOR_GYRO);
|
||||
}
|
||||
|
||||
|
||||
return gyroHardware;
|
||||
}
|
||||
|
||||
bool gyroInit(void)
|
||||
{
|
||||
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)
|
||||
gyroDev0.mpuIntExtiConfig = selectMPUIntExtiConfig();
|
||||
mpuDetect(&gyroDev0);
|
||||
mpuResetFn = gyroDev0.mpuConfiguration.resetFn;
|
||||
#endif
|
||||
const gyroSensor_e gyroHardware = gyroDetect(&gyroDev0);
|
||||
if (gyroHardware == GYRO_NONE) {
|
||||
return false;
|
||||
}
|
||||
|
||||
detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware;
|
||||
sensorsSet(SENSOR_GYRO);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool gyroInit(const gyroConfig_t *gyroConfigToUse)
|
||||
{
|
||||
gyroConfig = gyroConfigToUse;
|
||||
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)
|
||||
gyro.dev.mpuIntExtiConfig = selectMPUIntExtiConfig();
|
||||
mpuDetect(&gyro.dev);
|
||||
mpuReset = gyro.dev.mpuConfiguration.reset;
|
||||
#endif
|
||||
|
||||
if (!gyroDetect(&gyro.dev)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
switch (detectedSensors[SENSOR_INDEX_GYRO]) {
|
||||
default:
|
||||
// gyro does not support 32kHz
|
||||
// cast away constness, legitimate as this is cross-validation
|
||||
((gyroConfig_t*)gyroConfig)->gyro_use_32khz = false;
|
||||
break;
|
||||
switch (gyroHardware) {
|
||||
case GYRO_MPU6500:
|
||||
case GYRO_MPU9250:
|
||||
case GYRO_ICM20689:
|
||||
|
|
@ -258,12 +290,19 @@ bool gyroInit(const gyroConfig_t *gyroConfigToUse)
|
|||
case GYRO_ICM20602:
|
||||
// do nothing, as gyro supports 32kHz
|
||||
break;
|
||||
default:
|
||||
// gyro does not support 32kHz
|
||||
gyroConfigMutable()->gyro_use_32khz = false;
|
||||
break;
|
||||
}
|
||||
|
||||
// Must set gyro sample rate before initialisation
|
||||
gyro.targetLooptime = gyroSetSampleRate(&gyro.dev, gyroConfig->gyro_lpf, gyroConfig->gyro_sync_denom, gyroConfig->gyro_use_32khz);
|
||||
gyro.dev.lpf = gyroConfig->gyro_lpf;
|
||||
gyro.dev.init(&gyro.dev);
|
||||
gyro.targetLooptime = gyroSetSampleRate(&gyroDev0, gyroConfig()->gyro_lpf, gyroConfig()->gyro_sync_denom, gyroConfig()->gyro_use_32khz);
|
||||
gyroDev0.lpf = gyroConfig()->gyro_lpf;
|
||||
gyroDev0.init(&gyroDev0);
|
||||
if (gyroConfig()->gyro_align != ALIGN_DEFAULT) {
|
||||
gyroDev0.gyroAlign = gyroConfig()->gyro_align;
|
||||
}
|
||||
gyroInitFilters();
|
||||
return true;
|
||||
}
|
||||
|
|
@ -282,43 +321,43 @@ void gyroInitFilters(void)
|
|||
|
||||
uint32_t gyroFrequencyNyquist = (1.0f / (gyro.targetLooptime * 0.000001f)) / 2; // No rounding needed
|
||||
|
||||
if (gyroConfig->gyro_soft_lpf_hz && gyroConfig->gyro_soft_lpf_hz <= gyroFrequencyNyquist) { // Initialisation needs to happen once samplingrate is known
|
||||
if (gyroConfig->gyro_soft_lpf_type == FILTER_BIQUAD) {
|
||||
if (gyroConfig()->gyro_soft_lpf_hz && gyroConfig()->gyro_soft_lpf_hz <= gyroFrequencyNyquist) { // Initialisation needs to happen once samplingrate is known
|
||||
if (gyroConfig()->gyro_soft_lpf_type == FILTER_BIQUAD) {
|
||||
softLpfFilterApplyFn = (filterApplyFnPtr)biquadFilterApply;
|
||||
for (int axis = 0; axis < 3; axis++) {
|
||||
softLpfFilter[axis] = &gyroFilterLPF[axis];
|
||||
biquadFilterInitLPF(softLpfFilter[axis], gyroConfig->gyro_soft_lpf_hz, gyro.targetLooptime);
|
||||
biquadFilterInitLPF(softLpfFilter[axis], gyroConfig()->gyro_soft_lpf_hz, gyro.targetLooptime);
|
||||
}
|
||||
} else if (gyroConfig->gyro_soft_lpf_type == FILTER_PT1) {
|
||||
} else if (gyroConfig()->gyro_soft_lpf_type == FILTER_PT1) {
|
||||
softLpfFilterApplyFn = (filterApplyFnPtr)pt1FilterApply;
|
||||
const float gyroDt = (float) gyro.targetLooptime * 0.000001f;
|
||||
for (int axis = 0; axis < 3; axis++) {
|
||||
softLpfFilter[axis] = &gyroFilterPt1[axis];
|
||||
pt1FilterInit(softLpfFilter[axis], gyroConfig->gyro_soft_lpf_hz, gyroDt);
|
||||
pt1FilterInit(softLpfFilter[axis], gyroConfig()->gyro_soft_lpf_hz, gyroDt);
|
||||
}
|
||||
} else {
|
||||
softLpfFilterApplyFn = (filterApplyFnPtr)firFilterDenoiseUpdate;
|
||||
for (int axis = 0; axis < 3; axis++) {
|
||||
softLpfFilter[axis] = &gyroDenoiseState[axis];
|
||||
firFilterDenoiseInit(softLpfFilter[axis], gyroConfig->gyro_soft_lpf_hz, gyro.targetLooptime);
|
||||
firFilterDenoiseInit(softLpfFilter[axis], gyroConfig()->gyro_soft_lpf_hz, gyro.targetLooptime);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (gyroConfig->gyro_soft_notch_hz_1 && gyroConfig->gyro_soft_notch_hz_1 <= gyroFrequencyNyquist) {
|
||||
if (gyroConfig()->gyro_soft_notch_hz_1 && gyroConfig()->gyro_soft_notch_hz_1 <= gyroFrequencyNyquist) {
|
||||
notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply;
|
||||
const float gyroSoftNotchQ1 = filterGetNotchQ(gyroConfig->gyro_soft_notch_hz_1, gyroConfig->gyro_soft_notch_cutoff_1);
|
||||
const float gyroSoftNotchQ1 = filterGetNotchQ(gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1);
|
||||
for (int axis = 0; axis < 3; axis++) {
|
||||
notchFilter1[axis] = &gyroFilterNotch_1[axis];
|
||||
biquadFilterInit(notchFilter1[axis], gyroConfig->gyro_soft_notch_hz_1, gyro.targetLooptime, gyroSoftNotchQ1, FILTER_NOTCH);
|
||||
biquadFilterInit(notchFilter1[axis], gyroConfig()->gyro_soft_notch_hz_1, gyro.targetLooptime, gyroSoftNotchQ1, FILTER_NOTCH);
|
||||
}
|
||||
}
|
||||
if (gyroConfig->gyro_soft_notch_hz_2 && gyroConfig->gyro_soft_notch_hz_2 <= gyroFrequencyNyquist) {
|
||||
if (gyroConfig()->gyro_soft_notch_hz_2 && gyroConfig()->gyro_soft_notch_hz_2 <= gyroFrequencyNyquist) {
|
||||
notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
|
||||
const float gyroSoftNotchQ2 = filterGetNotchQ(gyroConfig->gyro_soft_notch_hz_2, gyroConfig->gyro_soft_notch_cutoff_2);
|
||||
const float gyroSoftNotchQ2 = filterGetNotchQ(gyroConfig()->gyro_soft_notch_hz_2, gyroConfig()->gyro_soft_notch_cutoff_2);
|
||||
for (int axis = 0; axis < 3; axis++) {
|
||||
notchFilter2[axis] = &gyroFilterNotch_2[axis];
|
||||
biquadFilterInit(notchFilter2[axis], gyroConfig->gyro_soft_notch_hz_2, gyro.targetLooptime, gyroSoftNotchQ2, FILTER_NOTCH);
|
||||
biquadFilterInit(notchFilter2[axis], gyroConfig()->gyro_soft_notch_hz_2, gyro.targetLooptime, gyroSoftNotchQ2, FILTER_NOTCH);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -348,7 +387,7 @@ void gyroSetCalibrationCycles(void)
|
|||
calibratingG = gyroCalculateCalibratingCycles();
|
||||
}
|
||||
|
||||
static void performGyroCalibration(uint8_t gyroMovementCalibrationThreshold)
|
||||
STATIC_UNIT_TESTED void performGyroCalibration(uint8_t gyroMovementCalibrationThreshold)
|
||||
{
|
||||
static int32_t g[3];
|
||||
static stdev_t var[3];
|
||||
|
|
@ -424,27 +463,27 @@ static bool gyroUpdateISR(gyroDev_t* gyroDev)
|
|||
void gyroUpdate(void)
|
||||
{
|
||||
// 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
|
||||
return;
|
||||
}
|
||||
if (!gyro.dev.read(&gyro.dev)) {
|
||||
if (!gyroDev0.read(&gyroDev0)) {
|
||||
return;
|
||||
}
|
||||
gyro.dev.dataReady = false;
|
||||
gyroDev0.dataReady = false;
|
||||
// move gyro data into 32-bit variables to avoid overflows in calculations
|
||||
gyroADC[X] = gyro.dev.gyroADCRaw[X];
|
||||
gyroADC[Y] = gyro.dev.gyroADCRaw[Y];
|
||||
gyroADC[Z] = gyro.dev.gyroADCRaw[Z];
|
||||
gyroADC[X] = gyroDev0.gyroADCRaw[X];
|
||||
gyroADC[Y] = gyroDev0.gyroADCRaw[Y];
|
||||
gyroADC[Z] = gyroDev0.gyroADCRaw[Z];
|
||||
|
||||
alignSensors(gyroADC, gyro.dev.gyroAlign);
|
||||
alignSensors(gyroADC, gyroDev0.gyroAlign);
|
||||
|
||||
const bool calibrationComplete = isGyroCalibrationComplete();
|
||||
if (calibrationComplete) {
|
||||
#if defined(GYRO_USES_SPI) && defined(USE_MPU_DATA_READY_SIGNAL)
|
||||
// SPI-based gyro so can read and update in ISR
|
||||
if (gyroConfig->gyro_isr_update) {
|
||||
mpuGyroSetIsrUpdate(&gyro.dev, gyroUpdateISR);
|
||||
if (gyroConfig()->gyro_isr_update) {
|
||||
mpuGyroSetIsrUpdate(&gyroDev0, gyroUpdateISR);
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
|
|
@ -452,13 +491,13 @@ void gyroUpdate(void)
|
|||
debug[3] = (uint16_t)(micros() & 0xffff);
|
||||
#endif
|
||||
} else {
|
||||
performGyroCalibration(gyroConfig->gyroMovementCalibrationThreshold);
|
||||
performGyroCalibration(gyroConfig()->gyroMovementCalibrationThreshold);
|
||||
}
|
||||
|
||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
gyroADC[axis] -= gyroZero[axis];
|
||||
// scale gyro output to degrees per second
|
||||
float gyroADCf = (float)gyroADC[axis] * gyro.dev.scale;
|
||||
float gyroADCf = (float)gyroADC[axis] * gyroDev0.scale;
|
||||
|
||||
// Apply LPF
|
||||
DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf));
|
||||
|
|
@ -472,8 +511,25 @@ void gyroUpdate(void)
|
|||
}
|
||||
|
||||
if (!calibrationComplete) {
|
||||
gyroADC[X] = lrintf(gyro.gyroADCf[X] / gyro.dev.scale);
|
||||
gyroADC[Y] = lrintf(gyro.gyroADCf[Y] / gyro.dev.scale);
|
||||
gyroADC[Z] = lrintf(gyro.gyroADCf[Z] / gyro.dev.scale);
|
||||
gyroADC[X] = lrintf(gyro.gyroADCf[X] / gyroDev0.scale);
|
||||
gyroADC[Y] = lrintf(gyro.gyroADCf[Y] / gyroDev0.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);
|
||||
}
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue