示例#1
0
文件: config.c 项目: npsm/inav
void activateConfig(void)
{
    static imuRuntimeConfig_t imuRuntimeConfig;

    activateControlRateConfig();

    resetAdjustmentStates();

    useRcControlsConfig(
        currentProfile->modeActivationConditions,
        &masterConfig.escAndServoConfig,
        &currentProfile->pidProfile
    );

    useGyroConfig(&masterConfig.gyroConfig, currentProfile->pidProfile.gyro_soft_lpf_hz);

#ifdef TELEMETRY
    telemetryUseConfig(&masterConfig.telemetryConfig);
#endif

    useFailsafeConfig(&masterConfig.failsafeConfig);

    setAccelerationZero(&masterConfig.accZero);
    setAccelerationGain(&masterConfig.accGain);
    setAccelerationFilter(currentProfile->pidProfile.acc_soft_lpf_hz);

    mixerUseConfigs(
#ifdef USE_SERVOS
        currentProfile->servoConf,
        &currentProfile->gimbalConfig,
#endif
        &masterConfig.flight3DConfig,
        &masterConfig.escAndServoConfig,
        &masterConfig.mixerConfig,
        &masterConfig.rxConfig
    );

    imuRuntimeConfig.dcm_kp_acc = masterConfig.dcm_kp_acc / 10000.0f;
    imuRuntimeConfig.dcm_ki_acc = masterConfig.dcm_ki_acc / 10000.0f;
    imuRuntimeConfig.dcm_kp_mag = masterConfig.dcm_kp_mag / 10000.0f;
    imuRuntimeConfig.dcm_ki_mag = masterConfig.dcm_ki_mag / 10000.0f;
    imuRuntimeConfig.small_angle = masterConfig.small_angle;

    imuConfigure(&imuRuntimeConfig, &currentProfile->pidProfile);

    pidInit();

#ifdef NAV
    navigationUseConfig(&masterConfig.navConfig);
    navigationUsePIDs(&currentProfile->pidProfile);
    navigationUseRcControlsConfig(&currentProfile->rcControlsConfig);
    navigationUseRxConfig(&masterConfig.rxConfig);
    navigationUseFlight3DConfig(&masterConfig.flight3DConfig);
    navigationUseEscAndServoConfig(&masterConfig.escAndServoConfig);
#endif

#ifdef BARO
    useBarometerConfig(&masterConfig.barometerConfig);
#endif
}
示例#2
0
void activateConfig(void)
{
    static imuRuntimeConfig_t imuRuntimeConfig;

    activateControlRateConfig();

    useRcControlsConfig(currentProfile->modeActivationConditions, &masterConfig.escAndServoConfig, &currentProfile->pidProfile);

    useGyroConfig(&masterConfig.gyroConfig);
#ifdef TELEMETRY
    useTelemetryConfig(&masterConfig.telemetryConfig);
#endif
    setPIDController(currentProfile->pidController);
#ifdef GPS
    gpsUseProfile(&currentProfile->gpsProfile);
    gpsUsePIDs(&currentProfile->pidProfile);
#endif
    useFailsafeConfig(&currentProfile->failsafeConfig);
    setAccelerationTrims(&masterConfig.accZero);
    mixerUseConfigs(
            currentProfile->servoConf,
            &masterConfig.flight3DConfig,
            &masterConfig.escAndServoConfig,
            &currentProfile->mixerConfig,
            &masterConfig.airplaneConfig,
            &masterConfig.rxConfig,
            &currentProfile->gimbalConfig
            );

    imuRuntimeConfig.gyro_cmpf_factor = masterConfig.gyro_cmpf_factor;
    imuRuntimeConfig.gyro_cmpfm_factor = masterConfig.gyro_cmpfm_factor;
    imuRuntimeConfig.acc_lpf_factor = currentProfile->acc_lpf_factor;
    imuRuntimeConfig.acc_unarmedcal = currentProfile->acc_unarmedcal;;
    imuRuntimeConfig.small_angle = masterConfig.small_angle;

    configureImu(&imuRuntimeConfig, &currentProfile->pidProfile, &currentProfile->accDeadband);
    configureAltitudeHold(&currentProfile->pidProfile, &currentProfile->barometerConfig, &currentProfile->rcControlsConfig, &masterConfig.escAndServoConfig);

    calculateThrottleAngleScale(currentProfile->throttle_correction_angle);
    calculateAccZLowPassFilterRCTimeConstant(currentProfile->accz_lpf_cutoff);

#ifdef BARO
    useBarometerConfig(&currentProfile->barometerConfig);
#endif
}
示例#3
0
void activateConfig(void)
{
    static imuRuntimeConfig_t imuRuntimeConfig;

    generatePitchCurve(&currentProfile.controlRateConfig);
    generateThrottleCurve(&currentProfile.controlRateConfig, &masterConfig.escAndServoConfig);

    useGyroConfig(&masterConfig.gyroConfig);
    useTelemetryConfig(&masterConfig.telemetryConfig);
    setPIDController(currentProfile.pidController);
#ifdef GPS
    gpsUseProfile(&currentProfile.gpsProfile);
    gpsUsePIDs(&currentProfile.pidProfile);
#endif
    useFailsafeConfig(&currentProfile.failsafeConfig);
    setAccelerationTrims(&masterConfig.accZero);
    mixerUseConfigs(
            currentProfile.servoConf,
            &masterConfig.flight3DConfig,
            &masterConfig.escAndServoConfig,
            &currentProfile.mixerConfig,
            &masterConfig.airplaneConfig,
            &masterConfig.rxConfig,
            &currentProfile.gimbalConfig
            );

    imuRuntimeConfig.gyro_cmpf_factor = masterConfig.gyro_cmpf_factor;
    imuRuntimeConfig.gyro_cmpfm_factor = masterConfig.gyro_cmpfm_factor;
    imuRuntimeConfig.acc_lpf_factor = currentProfile.acc_lpf_factor;
    imuRuntimeConfig.acc_unarmedcal = currentProfile.acc_unarmedcal;;
    imuRuntimeConfig.small_angle = masterConfig.small_angle;

    configureImu(&imuRuntimeConfig, &currentProfile.pidProfile, &currentProfile.barometerConfig, &currentProfile.accDeadband);

    calculateThrottleAngleScale(currentProfile.throttle_correction_angle);

#ifdef BARO
    useBarometerConfig(&currentProfile.barometerConfig);
#endif
}
示例#4
0
void activateConfig(void)
{
    static imuRuntimeConfig_t imuRuntimeConfig;

    activateControlRateConfig();

    resetAdjustmentStates();

    useRcControlsConfig(
        masterConfig.modeActivationConditions,
        &masterConfig.motorConfig,
        &currentProfile->pidProfile
    );

    // Prevent invalid notch cutoff
    if (masterConfig.gyro_soft_notch_cutoff_1 >= masterConfig.gyro_soft_notch_hz_1)
        masterConfig.gyro_soft_notch_hz_1 = 0;

    if (masterConfig.gyro_soft_notch_cutoff_2 >= masterConfig.gyro_soft_notch_hz_2)
        masterConfig.gyro_soft_notch_hz_2 = 0;

    gyroUseConfig(&masterConfig.gyroConfig,
        masterConfig.gyro_soft_lpf_hz,
        masterConfig.gyro_soft_notch_hz_1,
        masterConfig.gyro_soft_notch_cutoff_1,
        masterConfig.gyro_soft_notch_hz_2,
        masterConfig.gyro_soft_notch_cutoff_2,
        masterConfig.gyro_soft_type);

#ifdef TELEMETRY
    telemetryUseConfig(&masterConfig.telemetryConfig);
#endif

#ifdef GPS
    gpsUseProfile(&masterConfig.gpsProfile);
    gpsUsePIDs(&currentProfile->pidProfile);
#endif

    useFailsafeConfig(&masterConfig.failsafeConfig);
    setAccelerationTrims(&masterConfig.accZero);
    setAccelerationFilter(masterConfig.acc_lpf_hz);

    mixerUseConfigs(
        &masterConfig.flight3DConfig,
        &masterConfig.motorConfig,
        &masterConfig.mixerConfig,
        &masterConfig.airplaneConfig,
        &masterConfig.rxConfig
    );

#ifdef USE_SERVOS
    servoUseConfigs(&masterConfig.servoMixerConfig, masterConfig.servoConf, &masterConfig.gimbalConfig);
#endif

    imuRuntimeConfig.dcm_kp = masterConfig.dcm_kp / 10000.0f;
    imuRuntimeConfig.dcm_ki = masterConfig.dcm_ki / 10000.0f;
    imuRuntimeConfig.acc_unarmedcal = masterConfig.acc_unarmedcal;
    imuRuntimeConfig.small_angle = masterConfig.small_angle;

    imuConfigure(
        &imuRuntimeConfig,
        &currentProfile->pidProfile,
        &masterConfig.accDeadband,
        masterConfig.throttle_correction_angle
    );

    configureAltitudeHold(
        &currentProfile->pidProfile,
        &masterConfig.barometerConfig,
        &masterConfig.rcControlsConfig,
        &masterConfig.motorConfig
    );

#ifdef BARO
    useBarometerConfig(&masterConfig.barometerConfig);
#endif
}
示例#5
0
void activateConfig(void)
{
    static imuRuntimeConfig_t imuRuntimeConfig;

    activateControlRateConfig();

    resetAdjustmentStates();

    useRcControlsConfig(
        currentProfile->modeActivationConditions,
        &masterConfig.escAndServoConfig,
        &currentProfile->pidProfile
    );

    useGyroConfig(&masterConfig.gyroConfig);

#ifdef TELEMETRY
    useTelemetryConfig(&masterConfig.telemetryConfig);
#endif

    pidSetController(currentProfile->pidProfile.pidController);

#ifdef GPS
    gpsUseProfile(&currentProfile->gpsProfile);
    gpsUsePIDs(&currentProfile->pidProfile);
#endif

    useFailsafeConfig(&masterConfig.failsafeConfig);
    setAccelerationTrims(&masterConfig.accZero);

    mixerUseConfigs(
#ifdef USE_SERVOS
        currentProfile->servoConf,
        &currentProfile->gimbalConfig,
#endif
        &masterConfig.flight3DConfig,
        &masterConfig.escAndServoConfig,
        &masterConfig.mixerConfig,
        &masterConfig.airplaneConfig,
        &masterConfig.rxConfig
    );

    imuRuntimeConfig.gyro_cmpf_factor = masterConfig.gyro_cmpf_factor;
    imuRuntimeConfig.gyro_cmpfm_factor = masterConfig.gyro_cmpfm_factor;
    imuRuntimeConfig.acc_lpf_factor = currentProfile->acc_lpf_factor;
    imuRuntimeConfig.acc_unarmedcal = currentProfile->acc_unarmedcal;;
    imuRuntimeConfig.small_angle = masterConfig.small_angle;

    imuConfigure(
        &imuRuntimeConfig,
        &currentProfile->pidProfile,
        &currentProfile->accDeadband,
        currentProfile->accz_lpf_cutoff,
        currentProfile->throttle_correction_angle
    );

    configureAltitudeHold(
        &currentProfile->pidProfile,
        &currentProfile->barometerConfig,
        &currentProfile->rcControlsConfig,
        &masterConfig.escAndServoConfig
    );

#ifdef BARO
    useBarometerConfig(&currentProfile->barometerConfig);
#endif
}