Exemplo n.º 1
0
void targetConfiguration(void)
{
    if (getDetectedMotorType() == MOTOR_BRUSHED) {
        motorConfigMutable()->dev.motorPwmRate = BRUSHED_MOTORS_PWM_RATE;
        motorConfigMutable()->minthrottle = 1050; // for 6mm and 7mm brushed
    }

    /* Default to Spektrum */
    rxConfigMutable()->serialrx_provider = SERIALRX_SPEKTRUM2048; // all DSM* except DSM2 22ms
    rxConfigMutable()->spektrum_sat_bind = 5; // DSM2 11ms
    rxConfigMutable()->spektrum_sat_bind_autoreset = 1;
    rxConfigMutable()->mincheck = 1025;
    rxConfigMutable()->rcInterpolation = RC_SMOOTHING_MANUAL;
    rxConfigMutable()->rcInterpolationInterval = 14;
    parseRcChannels("TAER1234", rxConfigMutable());

    mixerConfigMutable()->yaw_motors_reversed = true;
    imuConfigMutable()->small_angle = 180;

    blackboxConfigMutable()->p_ratio = 128;

    /* Breadboard-specific settings for development purposes only
     */
#if defined(BREADBOARD)
    boardAlignmentMutable()->pitchDegrees = 90; // vertical breakout board
    barometerConfigMutable()->baro_hardware = BARO_DEFAULT; // still testing not on V1 or V2 pcb
#else
    barometerConfigMutable()->baro_hardware = BARO_NONE;
#endif

    compassConfigMutable()->mag_hardware =  MAG_NONE;

    systemConfigMutable()->cpu_overclock = 2; //216MHZ

    pidConfigMutable()->runaway_takeoff_prevention = false;

    featureEnable((FEATURE_DYNAMIC_FILTER | FEATURE_AIRMODE | FEATURE_ANTI_GRAVITY) ^ FEATURE_RX_PARALLEL_PWM);

    /* AlienWhoop PIDs tested with 6mm and 7mm motors on most frames */
    for (uint8_t pidProfileIndex = 0; pidProfileIndex < PID_PROFILE_COUNT; pidProfileIndex++) {
        pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex);

	pidProfile->pidSumLimit = 1000;
	pidProfile->pidSumLimitYaw = 1000;

        /* AlienWhoop PIDs tested with 6mm and 7mm motors on most frames */
        pidProfile->pid[PID_PITCH].P = 115;
        pidProfile->pid[PID_PITCH].I = 75;
        pidProfile->pid[PID_PITCH].D = 95;
        pidProfile->pid[PID_ROLL].P = 110;
        pidProfile->pid[PID_ROLL].I = 75;
        pidProfile->pid[PID_ROLL].D = 85;
        pidProfile->pid[PID_YAW].P = 220;
        pidProfile->pid[PID_YAW].I = 75;
        pidProfile->pid[PID_YAW].D = 20;
        pidProfile->pid[PID_LEVEL].P = 65;
        pidProfile->pid[PID_LEVEL].I = 65;
        pidProfile->pid[PID_LEVEL].D = 55;

        /* Setpoints */
        pidProfile->dterm_filter_type = FILTER_BIQUAD;
        pidProfile->dterm_notch_hz = 0;
        pidProfile->pid[PID_PITCH].F = 100;
        pidProfile->pid[PID_ROLL].F = 100;
        pidProfile->feedForwardTransition = 0;

	/* Anti-Gravity */
	pidProfile->itermThrottleThreshold = 500;
	pidProfile->itermAcceleratorGain = 5000;

	pidProfile->levelAngleLimit = 65;
    }

    for (uint8_t rateProfileIndex = 0; rateProfileIndex < CONTROL_RATE_PROFILE_COUNT; rateProfileIndex++) {
        controlRateConfig_t *controlRateConfig = controlRateProfilesMutable(rateProfileIndex);

        /* RC Rates */
        controlRateConfig->rcRates[FD_ROLL] = 218;
        controlRateConfig->rcRates[FD_PITCH] = 218;
        controlRateConfig->rcRates[FD_YAW] = 218;

	/* Classic Expo */
        controlRateConfig->rcExpo[FD_ROLL] = 45;
        controlRateConfig->rcExpo[FD_PITCH] = 45;
        controlRateConfig->rcExpo[FD_YAW] = 45;

        /* Super Expo Rates */
        controlRateConfig->rates[FD_ROLL] = 0;
        controlRateConfig->rates[FD_PITCH] = 0;
        controlRateConfig->rates[FD_YAW] = 0;

        /* Throttle PID Attenuation (TPA) */
        controlRateConfig->dynThrPID = 0; // tpa_rate off
        controlRateConfig->tpa_breakpoint = 1600;

	/* Force the clipping mixer at 100% seems better for brushed than default (off) and scaling)? */
        controlRateConfig->throttle_limit_type = THROTTLE_LIMIT_TYPE_CLIP;
        //controlRateConfig->throttle_limit_percent = 100;

        controlRateConfig->thrExpo8 = 20; // 20% throttle expo
    }
}
Exemplo n.º 2
0
static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int delta)
{
    if (delta > 0) {
        beeperConfirmationBeeps(2);
    } else {
        beeperConfirmationBeeps(1);
    }
    switch (adjustmentFunction) {
        case ADJUSTMENT_RC_EXPO:
            applyAdjustmentExpo(ADJUSTMENT_RC_EXPO, &controlRateConfig->stabilized.rcExpo8, delta);
            break;
        case ADJUSTMENT_RC_YAW_EXPO:
            applyAdjustmentExpo(ADJUSTMENT_RC_YAW_EXPO, &controlRateConfig->stabilized.rcYawExpo8, delta);
            break;
        case ADJUSTMENT_MANUAL_RC_EXPO:
            applyAdjustmentExpo(ADJUSTMENT_MANUAL_RC_EXPO, &controlRateConfig->manual.rcExpo8, delta);
            break;
        case ADJUSTMENT_MANUAL_RC_YAW_EXPO:
            applyAdjustmentExpo(ADJUSTMENT_MANUAL_RC_YAW_EXPO, &controlRateConfig->manual.rcYawExpo8, delta);
            break;
        case ADJUSTMENT_THROTTLE_EXPO:
            applyAdjustmentExpo(ADJUSTMENT_THROTTLE_EXPO, &controlRateConfig->throttle.rcExpo8, delta);
            break;
        case ADJUSTMENT_PITCH_ROLL_RATE:
        case ADJUSTMENT_PITCH_RATE:
            applyAdjustmentU8(ADJUSTMENT_PITCH_RATE, &controlRateConfig->stabilized.rates[FD_PITCH], delta, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
            if (adjustmentFunction == ADJUSTMENT_PITCH_RATE) {
                schedulePidGainsUpdate();
                break;
            }
            // follow though for combined ADJUSTMENT_PITCH_ROLL_RATE
            FALLTHROUGH;

        case ADJUSTMENT_ROLL_RATE:
            applyAdjustmentU8(ADJUSTMENT_ROLL_RATE, &controlRateConfig->stabilized.rates[FD_ROLL], delta, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
            schedulePidGainsUpdate();
            break;
        case ADJUSTMENT_MANUAL_PITCH_ROLL_RATE:
        case ADJUSTMENT_MANUAL_ROLL_RATE:
            applyAdjustmentManualRate(ADJUSTMENT_MANUAL_ROLL_RATE, &controlRateConfig->manual.rates[FD_ROLL], delta);
            if (adjustmentFunction == ADJUSTMENT_MANUAL_ROLL_RATE)
                break;
            // follow though for combined ADJUSTMENT_MANUAL_PITCH_ROLL_RATE
            FALLTHROUGH;
        case ADJUSTMENT_MANUAL_PITCH_RATE:
            applyAdjustmentManualRate(ADJUSTMENT_MANUAL_PITCH_RATE, &controlRateConfig->manual.rates[FD_PITCH], delta);
            break;
        case ADJUSTMENT_YAW_RATE:
            applyAdjustmentU8(ADJUSTMENT_YAW_RATE, &controlRateConfig->stabilized.rates[FD_YAW], delta, CONTROL_RATE_CONFIG_YAW_RATE_MIN, CONTROL_RATE_CONFIG_YAW_RATE_MAX);
            schedulePidGainsUpdate();
            break;
        case ADJUSTMENT_MANUAL_YAW_RATE:
            applyAdjustmentManualRate(ADJUSTMENT_MANUAL_YAW_RATE, &controlRateConfig->manual.rates[FD_YAW], delta);
            break;
        case ADJUSTMENT_PITCH_ROLL_P:
        case ADJUSTMENT_PITCH_P:
            applyAdjustmentPID(ADJUSTMENT_PITCH_P, &pidBankMutable()->pid[PID_PITCH].P, delta);
            if (adjustmentFunction == ADJUSTMENT_PITCH_P) {
                schedulePidGainsUpdate();
                break;
            }
            // follow though for combined ADJUSTMENT_PITCH_ROLL_P
            FALLTHROUGH;

        case ADJUSTMENT_ROLL_P:
            applyAdjustmentPID(ADJUSTMENT_ROLL_P, &pidBankMutable()->pid[PID_ROLL].P, delta);
            schedulePidGainsUpdate();
            break;
        case ADJUSTMENT_PITCH_ROLL_I:
        case ADJUSTMENT_PITCH_I:
            applyAdjustmentPID(ADJUSTMENT_PITCH_I, &pidBankMutable()->pid[PID_PITCH].I, delta);
            if (adjustmentFunction == ADJUSTMENT_PITCH_I) {
                schedulePidGainsUpdate();
                break;
            }
            // follow though for combined ADJUSTMENT_PITCH_ROLL_I
            FALLTHROUGH;

        case ADJUSTMENT_ROLL_I:
            applyAdjustmentPID(ADJUSTMENT_ROLL_I, &pidBankMutable()->pid[PID_ROLL].I, delta);
            schedulePidGainsUpdate();
            break;
        case ADJUSTMENT_PITCH_ROLL_D:
        case ADJUSTMENT_PITCH_D:
            applyAdjustmentPID(ADJUSTMENT_PITCH_D, &pidBankMutable()->pid[PID_PITCH].D, delta);
            if (adjustmentFunction == ADJUSTMENT_PITCH_D) {
                schedulePidGainsUpdate();
                break;
            }
            // follow though for combined ADJUSTMENT_PITCH_ROLL_D
            FALLTHROUGH;

        case ADJUSTMENT_ROLL_D:
            applyAdjustmentPID(ADJUSTMENT_ROLL_D, &pidBankMutable()->pid[PID_ROLL].D, delta);
            schedulePidGainsUpdate();
            break;
        case ADJUSTMENT_YAW_P:
            applyAdjustmentPID(ADJUSTMENT_YAW_P, &pidBankMutable()->pid[PID_YAW].P, delta);
            schedulePidGainsUpdate();
            break;
        case ADJUSTMENT_YAW_I:
            applyAdjustmentPID(ADJUSTMENT_YAW_I, &pidBankMutable()->pid[PID_YAW].I, delta);
            schedulePidGainsUpdate();
            break;
        case ADJUSTMENT_YAW_D:
            applyAdjustmentPID(ADJUSTMENT_YAW_D, &pidBankMutable()->pid[PID_YAW].D, delta);
            schedulePidGainsUpdate();
            break;
        case ADJUSTMENT_NAV_FW_CRUISE_THR:
            applyAdjustmentU16(ADJUSTMENT_NAV_FW_CRUISE_THR, &navConfigMutable()->fw.cruise_throttle, delta, 1000, 2000);
            break;
        case ADJUSTMENT_NAV_FW_PITCH2THR:
            applyAdjustmentU8(ADJUSTMENT_NAV_FW_PITCH2THR, &navConfigMutable()->fw.pitch_to_throttle, delta, 0, 100);
            break;
        case ADJUSTMENT_ROLL_BOARD_ALIGNMENT:
            updateBoardAlignment(delta, 0);
            blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_BOARD_ALIGNMENT, boardAlignment()->rollDeciDegrees);
            break;
        case ADJUSTMENT_PITCH_BOARD_ALIGNMENT:
            updateBoardAlignment(0, delta);
            blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_BOARD_ALIGNMENT, boardAlignment()->pitchDeciDegrees);
            break;
        case ADJUSTMENT_LEVEL_P:
            applyAdjustmentPID(ADJUSTMENT_LEVEL_P, &pidBankMutable()->pid[PID_LEVEL].P, delta);
            // TODO: Need to call something to take it into account?
            break;
        case ADJUSTMENT_LEVEL_I:
            applyAdjustmentPID(ADJUSTMENT_LEVEL_I, &pidBankMutable()->pid[PID_LEVEL].I, delta);
            // TODO: Need to call something to take it into account?
            break;
        case ADJUSTMENT_LEVEL_D:
            applyAdjustmentPID(ADJUSTMENT_LEVEL_D, &pidBankMutable()->pid[PID_LEVEL].D, delta);
            // TODO: Need to call something to take it into account?
            break;
        case ADJUSTMENT_POS_XY_P:
            applyAdjustmentPID(ADJUSTMENT_POS_XY_P, &pidBankMutable()->pid[PID_POS_XY].P, delta);
            navigationUsePIDs();
            break;
        case ADJUSTMENT_POS_XY_I:
            applyAdjustmentPID(ADJUSTMENT_POS_XY_I, &pidBankMutable()->pid[PID_POS_XY].I, delta);
            navigationUsePIDs();
            break;
        case ADJUSTMENT_POS_XY_D:
            applyAdjustmentPID(ADJUSTMENT_POS_XY_D, &pidBankMutable()->pid[PID_POS_XY].D, delta);
            navigationUsePIDs();
            break;
        case ADJUSTMENT_POS_Z_P:
            applyAdjustmentPID(ADJUSTMENT_POS_Z_P, &pidBankMutable()->pid[PID_POS_Z].P, delta);
            navigationUsePIDs();
            break;
        case ADJUSTMENT_POS_Z_I:
            applyAdjustmentPID(ADJUSTMENT_POS_Z_I, &pidBankMutable()->pid[PID_POS_Z].I, delta);
            navigationUsePIDs();
            break;
        case ADJUSTMENT_POS_Z_D:
            applyAdjustmentPID(ADJUSTMENT_POS_Z_D, &pidBankMutable()->pid[PID_POS_Z].D, delta);
            navigationUsePIDs();
            break;
        case ADJUSTMENT_HEADING_P:
            applyAdjustmentPID(ADJUSTMENT_HEADING_P, &pidBankMutable()->pid[PID_HEADING].P, delta);
            // TODO: navigationUsePIDs()?
            break;
        case ADJUSTMENT_VEL_XY_P:
            applyAdjustmentPID(ADJUSTMENT_VEL_XY_P, &pidBankMutable()->pid[PID_VEL_XY].P, delta);
            navigationUsePIDs();
            break;
        case ADJUSTMENT_VEL_XY_I:
            applyAdjustmentPID(ADJUSTMENT_VEL_XY_I, &pidBankMutable()->pid[PID_VEL_XY].I, delta);
            navigationUsePIDs();
            break;
        case ADJUSTMENT_VEL_XY_D:
            applyAdjustmentPID(ADJUSTMENT_VEL_XY_D, &pidBankMutable()->pid[PID_VEL_XY].D, delta);
            navigationUsePIDs();
            break;
        case ADJUSTMENT_VEL_Z_P:
            applyAdjustmentPID(ADJUSTMENT_VEL_Z_P, &pidBankMutable()->pid[PID_VEL_Z].P, delta);
            navigationUsePIDs();
            break;
        case ADJUSTMENT_VEL_Z_I:
            applyAdjustmentPID(ADJUSTMENT_VEL_Z_I, &pidBankMutable()->pid[PID_VEL_Z].I, delta);
            navigationUsePIDs();
            break;
        case ADJUSTMENT_VEL_Z_D:
            applyAdjustmentPID(ADJUSTMENT_VEL_Z_D, &pidBankMutable()->pid[PID_VEL_Z].D, delta);
            navigationUsePIDs();
            break;
        case ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE:
            applyAdjustmentU16(ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE, &mixerConfigMutable()->fwMinThrottleDownPitchAngle, delta, 0, FW_MIN_THROTTLE_DOWN_PITCH_ANGLE_MAX);
            break;
        default:
            break;
    };
}
Exemplo n.º 3
0
static void validateAndFixConfig(void)
{
#if !defined(USE_QUAD_MIXER_ONLY) && !defined(USE_OSD_SLAVE)
    // Reset unsupported mixer mode to default.
    // This check will be gone when motor/servo mixers are loaded dynamically
    // by configurator as a part of configuration procedure.

    mixerMode_e mixerMode = mixerConfigMutable()->mixerMode;

    if (!(mixerMode == MIXER_CUSTOM || mixerMode == MIXER_CUSTOM_AIRPLANE || mixerMode == MIXER_CUSTOM_TRI)) {
        if (mixers[mixerMode].motorCount && mixers[mixerMode].motor == NULL)
            mixerConfigMutable()->mixerMode = MIXER_CUSTOM;
#ifdef USE_SERVOS
        if (mixers[mixerMode].useServo && servoMixers[mixerMode].servoRuleCount == 0)
            mixerConfigMutable()->mixerMode = MIXER_CUSTOM_AIRPLANE;
#endif
    }
#endif

#ifndef USE_OSD_SLAVE
    if (systemConfig()->activeRateProfile >= CONTROL_RATE_PROFILE_COUNT) {
        systemConfigMutable()->activeRateProfile = 0;
    }
    setControlRateProfile(systemConfig()->activeRateProfile);

    if (systemConfig()->pidProfileIndex >= MAX_PROFILE_COUNT) {
        systemConfigMutable()->pidProfileIndex = 0;
    }
    setPidProfile(systemConfig()->pidProfileIndex);

    // Prevent invalid notch cutoff
    if (currentPidProfile->dterm_notch_cutoff >= currentPidProfile->dterm_notch_hz) {
        currentPidProfile->dterm_notch_hz = 0;
    }

    if ((motorConfig()->dev.motorPwmProtocol == PWM_TYPE_BRUSHED) && (motorConfig()->mincommand < 1000)) {
        motorConfigMutable()->mincommand = 1000;
    }

    if ((motorConfig()->dev.motorPwmProtocol == PWM_TYPE_STANDARD) && (motorConfig()->dev.motorPwmRate > BRUSHLESS_MOTORS_PWM_RATE)) {
        motorConfigMutable()->dev.motorPwmRate = BRUSHLESS_MOTORS_PWM_RATE;
    }

    validateAndFixGyroConfig();

    if (!(featureConfigured(FEATURE_RX_PARALLEL_PWM) || featureConfigured(FEATURE_RX_PPM) || featureConfigured(FEATURE_RX_SERIAL) || featureConfigured(FEATURE_RX_MSP) || featureConfigured(FEATURE_RX_SPI))) {
        featureSet(DEFAULT_RX_FEATURE);
    }

    if (featureConfigured(FEATURE_RX_PPM)) {
        featureClear(FEATURE_RX_SERIAL | FEATURE_RX_PARALLEL_PWM | FEATURE_RX_MSP | FEATURE_RX_SPI);
    }

    if (featureConfigured(FEATURE_RX_MSP)) {
        featureClear(FEATURE_RX_SERIAL | FEATURE_RX_PARALLEL_PWM | FEATURE_RX_PPM | FEATURE_RX_SPI);
    }

    if (featureConfigured(FEATURE_RX_SERIAL)) {
        featureClear(FEATURE_RX_PARALLEL_PWM | FEATURE_RX_MSP | FEATURE_RX_PPM | FEATURE_RX_SPI);
    }

#ifdef USE_RX_SPI
    if (featureConfigured(FEATURE_RX_SPI)) {
        featureClear(FEATURE_RX_SERIAL | FEATURE_RX_PARALLEL_PWM | FEATURE_RX_PPM | FEATURE_RX_MSP);
    }
#endif // USE_RX_SPI

    if (featureConfigured(FEATURE_RX_PARALLEL_PWM)) {
        featureClear(FEATURE_RX_SERIAL | FEATURE_RX_MSP | FEATURE_RX_PPM | FEATURE_RX_SPI);
#if defined(STM32F10X)
        // rssi adc needs the same ports
        featureClear(FEATURE_RSSI_ADC);
        // current meter needs the same ports
        if (batteryConfig()->currentMeterSource == CURRENT_METER_ADC) {
            batteryConfigMutable()->currentMeterSource = CURRENT_METER_NONE;
        }
#endif // STM32F10X
        // software serial needs free PWM ports
        featureClear(FEATURE_SOFTSERIAL);
    }

#ifdef USE_SOFTSPI
    if (featureConfigured(FEATURE_SOFTSPI)) {
        featureClear(FEATURE_RX_PPM | FEATURE_RX_PARALLEL_PWM | FEATURE_SOFTSERIAL);
        batteryConfigMutable()->voltageMeterSource = VOLTAGE_METER_NONE;
#if defined(STM32F10X)
        featureClear(FEATURE_LED_STRIP);
        // rssi adc needs the same ports
        featureClear(FEATURE_RSSI_ADC);
        // current meter needs the same ports
        if (batteryConfig()->currentMeterSource == CURRENT_METER_ADC) {
            batteryConfigMutable()->currentMeterSource = CURRENT_METER_NONE;
        }
#endif // STM32F10X
    }
#endif // USE_SOFTSPI

#endif // USE_OSD_SLAVE

    if (!isSerialConfigValid(serialConfig())) {
        pgResetFn_serialConfig(serialConfigMutable());
    }

// clear features that are not supported.
// I have kept them all here in one place, some could be moved to sections of code above.

#ifndef USE_PPM
    featureClear(FEATURE_RX_PPM);
#endif

#ifndef USE_SERIAL_RX
    featureClear(FEATURE_RX_SERIAL);
#endif

#if !defined(USE_SOFTSERIAL1) && !defined(USE_SOFTSERIAL2)
    featureClear(FEATURE_SOFTSERIAL);
#endif

#ifndef USE_GPS
    featureClear(FEATURE_GPS);
#endif

#ifndef USE_RANGEFINDER
    featureClear(FEATURE_RANGEFINDER);
#endif

#ifndef USE_TELEMETRY
    featureClear(FEATURE_TELEMETRY);
#endif

#ifndef USE_PWM
    featureClear(FEATURE_RX_PARALLEL_PWM);
#endif

#ifndef USE_RX_MSP
    featureClear(FEATURE_RX_MSP);
#endif

#ifndef USE_LED_STRIP
    featureClear(FEATURE_LED_STRIP);
#endif

#ifndef USE_DASHBOARD
    featureClear(FEATURE_DASHBOARD);
#endif

#ifndef USE_OSD
    featureClear(FEATURE_OSD);
#endif

#ifndef USE_SERVOS
    featureClear(FEATURE_SERVO_TILT | FEATURE_CHANNEL_FORWARDING);
#endif

#ifndef USE_TRANSPONDER
    featureClear(FEATURE_TRANSPONDER);
#endif

#ifndef USE_RX_SPI
    featureClear(FEATURE_RX_SPI);
#endif

#ifndef USE_SOFTSPI
    featureClear(FEATURE_SOFTSPI);
#endif

#ifndef USE_ESC_SENSOR
    featureClear(FEATURE_ESC_SENSOR);
#endif

#ifndef USE_GYRO_DATA_ANALYSE
    featureClear(FEATURE_DYNAMIC_FILTER);
#endif

#if defined(TARGET_VALIDATECONFIG)
    targetValidateConfiguration();
#endif
}