betaflight的陀螺仪初始化

在init函数里有一个判断

    if (!sensorsAutodetect()) {
        // if gyro was not detected due to whatever reason, notify and don't arm.
        indicateFailure(FAILURE_MISSING_ACC, 2);
        setArmingDisabled(ARMING_DISABLED_NO_GYRO);
    }

打开sensorsAutodetect就有

bool sensorsAutodetect(void)
{

    // gyro must be initialised before accelerometer

    bool gyroDetected = gyroInit();

    if (gyroDetected) {
        accInit(gyro.targetLooptime);
    }

#ifdef USE_MAG
    compassInit();
#endif

#ifdef USE_BARO
    baroDetect(&baro.dev, barometerConfig()->baro_hardware);
#endif

#ifdef USE_RANGEFINDER
    rangefinderInit();
#endif

    return gyroDetected;
}

在这里初始化了陀螺仪，就有

bool gyroInit(void)
{
#ifdef USE_GYRO_OVERFLOW_CHECK//有定义
    if (gyroConfig()->checkOverflow == GYRO_OVERFLOW_CHECK_YAW) {
        overflowAxisMask = GYRO_OVERFLOW_Z;
    } else if (gyroConfig()->checkOverflow == GYRO_OVERFLOW_CHECK_ALL_AXES) {
            overflowAxisMask = GYRO_OVERFLOW_X | GYRO_OVERFLOW_Y | GYRO_OVERFLOW_Z;
            //刚开始这里设置了.checkOverflow=GYRO_OVERFLOW_CHECK_ALL_AXES
    } else {
        overflowAxisMask = 0;
    }
    //就有overflowAxisMask = GYRO_OVERFLOW_X | GYRO_OVERFLOW_Y | GYRO_OVERFLOW_Z;
#endif

    switch (debugMode) {
    case DEBUG_FFT:
    case DEBUG_GYRO_NOTCH:
    case DEBUG_GYRO:
    case DEBUG_GYRO_RAW:
        gyroDebugMode = debugMode;
        break;
    default:
        // debugMode is not gyro-related
        gyroDebugMode = DEBUG_NONE;
        break;
    }
    //设置debug模式
    firstArmingCalibrationWasStarted = false;
    memset(&gyro, 0, sizeof(gyro));
    return gyroInitSensor(&gyroSensor1);
}

下面进入

static bool gyroInitSensor(gyroSensor_t *gyroSensor)
{
#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_ICM20601) || defined(USE_GYRO_SPI_ICM20649) || defined(USE_GYRO_SPI_ICM20689)
//mpu6050
#if defined(MPU_INT_EXTI)//没定义
    gyroSensor->gyroDev.mpuIntExtiTag =  IO_TAG(MPU_INT_EXTI);
#elif defined(USE_HARDWARE_REVISION_DETECTION)//没定义
    gyroSensor->gyroDev.mpuIntExtiTag =  selectMPUIntExtiConfigByHardwareRevision();
#else
    gyroSensor->gyroDev.mpuIntExtiTag =  IO_TAG_NONE;//只有这一个
#endif // MPU_INT_EXTI

#ifdef USE_DUAL_GYRO//没定义
    // set cnsPin using GYRO_n_CS_PIN defined in target.h
    gyroSensor->gyroDev.bus.busdev_u.spi.csnPin = gyroConfig()->gyro_to_use == 0 ? IOGetByTag(IO_TAG(GYRO_0_CS_PIN)) : IOGetByTag(IO_TAG(GYRO_1_CS_PIN));
#else
    gyroSensor->gyroDev.bus.busdev_u.spi.csnPin = IO_NONE; // set cnsPin to IO_NONE so mpuDetect will set it according to value defined in target.h
#endif // USE_DUAL_GYRO
    mpuDetect(&gyroSensor->gyroDev);
    mpuResetFn = gyroSensor->gyroDev.mpuConfiguration.resetFn; // must be set after mpuDetect
#endif
    gyroSensor->gyroDev.gyro_high_fsr = gyroConfig()->gyro_high_fsr;

    const gyroSensor_e gyroHardware = gyroDetect(&gyroSensor->gyroDev);
    if (gyroHardware == GYRO_NONE) {
        return false;
    }

    switch (gyroHardware) {
    case GYRO_MPU6500:
    case GYRO_MPU9250:
    case GYRO_ICM20601:
    case GYRO_ICM20602:
    case GYRO_ICM20608G:
    case GYRO_ICM20689:
        // do nothing, as gyro supports 32kHz
        break;
    default:
        // gyro does not support 32kHz
        gyroConfigMutable()->gyro_use_32khz = false;
        break;
    }

    // Must set gyro targetLooptime before gyroDev.init and initialisation of filters
    gyro.targetLooptime = gyroSetSampleRate(&gyroSensor->gyroDev, gyroConfig()->gyro_lpf, gyroConfig()->gyro_sync_denom, gyroConfig()->gyro_use_32khz);//采样率设置
    gyroSensor->gyroDev.lpf = gyroConfig()->gyro_lpf;//低通滤波
    gyroSensor->gyroDev.initFn(&gyroSensor->gyroDev);//这定义个函数指针处理了很多东西
    if (gyroConfig()->gyro_align != ALIGN_DEFAULT) {
        gyroSensor->gyroDev.gyroAlign = gyroConfig()->gyro_align;
    }

    gyroInitSensorFilters(gyroSensor);
#ifdef USE_GYRO_DATA_ANALYSE//有定义
    gyroDataAnalyseInit(gyro.targetLooptime);
#endif
    return true;
}

至此初始化完毕有一些细节
首先 mpuDetect判断mpu
mpuDetect(&gyroSensor1->gyroDev)

void mpuDetect(gyroDev_t *gyro)
{
    // MPU datasheet specifies 30ms.
    delay(35);

#ifdef USE_I2C//有定义
    gyro->bus.bustype = BUSTYPE_I2C;
    gyro->bus.busdev_u.i2c.device = MPU_I2C_INSTANCE;
    gyro->bus.busdev_u.i2c.address = MPU_ADDRESS;
    uint8_t sig = 0;
    bool ack = busReadRegisterBuffer(&gyro->bus, MPU_RA_WHO_AM_I, &sig, 1);
#else
    bool ack = false;
#endif

    if (!ack) {
#ifdef USE_SPI//没有
        detectSPISensorsAndUpdateDetectionResult(gyro);
#endif
        return;
    }

#ifdef USE_I2C
    // If an MPU3050 is connected sig will contain 0.
    uint8_t inquiryResult;
    ack = busReadRegisterBuffer(&gyro->bus, MPU_RA_WHO_AM_I_LEGACY, &inquiryResult, 1);
    inquiryResult &= MPU_INQUIRY_MASK;
    //判断是不是用的是mpu3050，不是
    if (ack && inquiryResult == MPUx0x0_WHO_AM_I_CONST) {
        gyro->mpuDetectionResult.sensor = MPU_3050;
        return;
    }

    sig &= MPU_INQUIRY_MASK;
    if (sig == MPUx0x0_WHO_AM_I_CONST) {
        //进入这个
        gyro->mpuDetectionResult.sensor = MPU_60x0;
        mpu6050FindRevision(gyro);
    } else if (sig == MPU6500_WHO_AM_I_CONST) {
        gyro->mpuDetectionResult.sensor = MPU_65xx_I2C;
    }
#endif
}

i2c读

bool busReadRegisterBuffer(const busDevice_t *busdev, uint8_t reg, uint8_t *data, uint8_t length)
{
#if !defined(USE_SPI) && !defined(USE_I2C)
    UNUSED(reg);
    UNUSED(data);
    UNUSED(length);
#endif
    switch (busdev->bustype) {
#ifdef USE_SPI
    case BUSTYPE_SPI:
        return spiBusReadRegisterBuffer(busdev, reg | 0x80, data, length);
#endif
#ifdef USE_I2C
    case BUSTYPE_I2C:
        return i2cBusReadRegisterBuffer(busdev, reg, data, length);
#endif
    default:
        return false;
    }
}

返回return i2cBusReadRegisterBuffer(&gyro->bus, MPU_RA_WHO_AM_I, &sig, 1);

bool i2cBusReadRegisterBuffer(const busDevice_t *busdev, uint8_t reg, uint8_t *data, uint8_t length)
{
    return i2cRead(busdev->busdev_u.i2c.device, busdev->busdev_u.i2c.address, reg, length, data);
}

读数据
下面回到
const gyroSensor_e gyroHardware = gyroDetect(&gyroSensor->gyroDev);

STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev)
{
    gyroSensor_e gyroHardware = GYRO_DEFAULT;

    dev->gyroAlign = ALIGN_DEFAULT;

    switch (gyroHardware) {
    case GYRO_DEFAULT:
        FALLTHROUGH;

#ifdef USE_GYRO_MPU6050
    case GYRO_MPU6050:
        if (mpu6050GyroDetect(dev)) {
            gyroHardware = GYRO_MPU6050;
#ifdef GYRO_MPU6050_ALIGN
            dev->gyroAlign = GYRO_MPU6050_ALIGN;
#endif
            break;
        }
        FALLTHROUGH;
#endif

#ifdef USE_GYRO_L3G4200D
    case GYRO_L3G4200D:
        if (l3g4200dDetect(dev)) {
            gyroHardware = GYRO_L3G4200D;
#ifdef GYRO_L3G4200D_ALIGN
            dev->gyroAlign = GYRO_L3G4200D_ALIGN;
#endif
            break;
        }
        FALLTHROUGH;
#endif

#ifdef USE_GYRO_MPU3050
    case GYRO_MPU3050:
        if (mpu3050Detect(dev)) {
            gyroHardware = GYRO_MPU3050;
#ifdef GYRO_MPU3050_ALIGN
            dev->gyroAlign = GYRO_MPU3050_ALIGN;
#endif
            break;
        }
        FALLTHROUGH;
#endif

#ifdef USE_GYRO_L3GD20
    case GYRO_L3GD20:
        if (l3gd20Detect(dev)) {
            gyroHardware = GYRO_L3GD20;
#ifdef GYRO_L3GD20_ALIGN
            dev->gyroAlign = GYRO_L3GD20_ALIGN;
#endif
            break;
        }
        FALLTHROUGH;
#endif

#ifdef USE_GYRO_SPI_MPU6000
    case GYRO_MPU6000:
        if (mpu6000SpiGyroDetect(dev)) {
            gyroHardware = GYRO_MPU6000;
#ifdef GYRO_MPU6000_ALIGN
            dev->gyroAlign = GYRO_MPU6000_ALIGN;
#endif
            break;
        }
        FALLTHROUGH;
#endif

#if defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500)
    case GYRO_MPU6500:
    case GYRO_ICM20601:
    case GYRO_ICM20602:
    case GYRO_ICM20608G:
#ifdef USE_GYRO_SPI_MPU6500
        if (mpu6500GyroDetect(dev) || mpu6500SpiGyroDetect(dev)) {
#else
        if (mpu6500GyroDetect(dev)) {
#endif
            switch (dev->mpuDetectionResult.sensor) {
            case MPU_9250_SPI:
                gyroHardware = GYRO_MPU9250;
                break;
            case ICM_20601_SPI:
                gyroHardware = GYRO_ICM20601;
                break;
            case ICM_20602_SPI:
                gyroHardware = GYRO_ICM20602;
                break;
            case ICM_20608_SPI:
                gyroHardware = GYRO_ICM20608G;
                break;
            default:
                gyroHardware = GYRO_MPU6500;
            }
#ifdef GYRO_MPU6500_ALIGN
            dev->gyroAlign = GYRO_MPU6500_ALIGN;
#endif
            break;
        }
        FALLTHROUGH;
#endif

#ifdef USE_GYRO_SPI_MPU9250
    case GYRO_MPU9250:
        if (mpu9250SpiGyroDetect(dev)) {
            gyroHardware = GYRO_MPU9250;
#ifdef GYRO_MPU9250_ALIGN
            dev->gyroAlign = GYRO_MPU9250_ALIGN;
#endif
            break;
        }
        FALLTHROUGH;
#endif

#ifdef USE_GYRO_SPI_ICM20649
    case GYRO_ICM20649:
        if (icm20649SpiGyroDetect(dev)) {
            gyroHardware = GYRO_ICM20649;
#ifdef GYRO_ICM20649_ALIGN
            dev->gyroAlign = GYRO_ICM20649_ALIGN;
#endif
            break;
        }
        FALLTHROUGH;
#endif

#ifdef USE_GYRO_SPI_ICM20689
    case GYRO_ICM20689:
        if (icm20689SpiGyroDetect(dev)) {
            gyroHardware = GYRO_ICM20689;
#ifdef GYRO_ICM20689_ALIGN
            dev->gyroAlign = GYRO_ICM20689_ALIGN;
#endif
            break;
        }
        FALLTHROUGH;
#endif

#ifdef USE_ACCGYRO_BMI160
    case GYRO_BMI160:
        if (bmi160SpiGyroDetect(dev)) {
            gyroHardware = GYRO_BMI160;
#ifdef GYRO_BMI160_ALIGN
            dev->gyroAlign = GYRO_BMI160_ALIGN;
#endif
            break;
        }
        FALLTHROUGH;
#endif

#ifdef USE_FAKE_GYRO
    case GYRO_FAKE:
        if (fakeGyroDetect(dev)) {
            gyroHardware = GYRO_FAKE;
            break;
        }
        FALLTHROUGH;
#endif

    default:
        gyroHardware = GYRO_NONE;
    }

    if (gyroHardware != GYRO_NONE) {
        detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware;
        sensorsSet(SENSOR_GYRO);
    }


    return gyroHardware;
}

这里得到
gyroHardware = GYRO_MPU6050;
dev->gyroAlign = GYRO_MPU6050_ALIGN;
这里有调用函数mpu6050GyroDetect赋值

bool mpu6050GyroDetect(gyroDev_t *gyro)
{
    if (gyro->mpuDetectionResult.sensor != MPU_60x0) {
        return false;
    }
    gyro->initFn = mpu6050GyroInit;
    gyro->readFn = mpuGyroRead;

    // 16.4 dps/lsb scalefactor
    gyro->scale = 1.0f / 16.4f;

    return true;
}

下面回到gyroInitSensor里的
gyro.targetLooptime = gyroSetSampleRate(&gyroSensor->gyroDev, gyroConfig()->gyro_lpf, gyroConfig()->gyro_sync_denom, gyroConfig()->gyro_use_32khz);

uint32_t gyroSetSampleRate(gyroDev_t *gyro, uint8_t lpf, uint8_t gyroSyncDenominator, bool gyro_use_32khz)
{                              //&gyroSensor->gyroDev, gyroConfig()->gyro_lpf, gyroConfig()->gyro_sync_denom, gyroConfig()->gyro_use_32khz
    float gyroSamplePeriod;

    if (lpf == GYRO_LPF_256HZ || lpf == GYRO_LPF_NONE) {
        if (gyro_use_32khz) {
            gyro->gyroRateKHz = GYRO_RATE_32_kHz;//flase
            gyroSamplePeriod = 31.5f;
        } else {
            switch (gyro->mpuDetectionResult.sensor) {
            case BMI_160_SPI:
                gyro->gyroRateKHz = GYRO_RATE_3200_Hz;
                gyroSamplePeriod = 312.0f;
                break;
            case ICM_20649_SPI:
                gyro->gyroRateKHz = GYRO_RATE_9_kHz;
                gyroSamplePeriod = 1000000.0f / 9000.0f;
                break;
            default:
                //进入这个
                gyro->gyroRateKHz = GYRO_RATE_8_kHz;
                gyroSamplePeriod = 125.0f;
                break;
            }
        }
    } else {
        switch (gyro->mpuDetectionResult.sensor) {
        case ICM_20649_SPI:
            gyro->gyroRateKHz = GYRO_RATE_1100_Hz;
            gyroSamplePeriod = 1000000.0f / 1100.0f;
            break;
        default:
            gyro->gyroRateKHz = GYRO_RATE_1_kHz;
            gyroSamplePeriod = 1000.0f;
            break;
        }
        gyroSyncDenominator = 1; // Always full Sampling 1khz
    }

    // calculate gyro divider and targetLooptime (expected cycleTime)
    gyro->mpuDividerDrops  = gyroSyncDenominator - 1;//4-1=3
    const uint32_t targetLooptime = (uint32_t)(gyroSyncDenominator * gyroSamplePeriod);
    return targetLooptime;//375
}

接着就有gyroInitSensorFilters：

static void gyroInitSensorFilters(gyroSensor_t *gyroSensor)
{
#if defined(USE_GYRO_SLEW_LIMITER)//有定义
    gyroInitSlewLimiter(gyroSensor);
#endif
    gyroInitFilterLpf(gyroSensor, gyroConfig()->gyro_soft_lpf_hz);
    gyroInitFilterNotch1(gyroSensor, gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1);
    gyroInitFilterNotch2(gyroSensor, gyroConfig()->gyro_soft_notch_hz_2, gyroConfig()->gyro_soft_notch_cutoff_2);
#ifdef USE_GYRO_DATA_ANALYSE//有定义
    gyroInitFilterDynamicNotch(gyroSensor);
#endif
}

接着初始化gyroDataAnalyseInit(gyro.targetLooptime);

void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
{
    // initialise even if FEATURE_DYNAMIC_FILTER not set, since it may be set later
    samplingFrequency = 1000000 / targetLooptimeUs;
    fftSamplingScale = samplingFrequency / FFT_SAMPLING_RATE;
    fftMaxFreq = FFT_SAMPLING_RATE / 2;
    fftBinCount = fftFreqToBin(fftMaxFreq) + 1;
    fftResolution = FFT_SAMPLING_RATE / FFT_WINDOW_SIZE;
    arm_rfft_fast_init_f32(&fftInstance, FFT_WINDOW_SIZE);

    initGyroData();
    initHanning();

    // recalculation of filters takes 4 calls per axis => each filter gets updated every 3 * 4 = 12 calls
    // at 4khz gyro loop rate this means 4khz / 4 / 3 = 333Hz => update every 3ms
    float looptime = targetLooptimeUs * 4 * 3;
    for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
        fftResult[axis].centerFreq = 200; // any init value
        biquadFilterInitLPF(&fftFreqFilter[axis], DYN_NOTCH_CHANGERATE, looptime);
        biquadFilterInit(&fftGyroFilter[axis], FFT_BPF_HZ, 1000000 / FFT_SAMPLING_RATE, BIQUAD_Q, FILTER_BPF);
    }
}