Esempio n. 1
0
void autotuneUpdateGains(pidAutotuneData_t * data)
{
    for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
        pidBankMutable()->pid[axis].P = lrintf(data[axis].gainP);
        pidBankMutable()->pid[axis].I = lrintf(data[axis].gainI);
        pidBankMutable()->pid[axis].D = lrintf(data[axis].gainD);
    }
    schedulePidGainsUpdate();
}
Esempio n. 2
0
static long cmsx_PidWriteback(const OSD_Entry *self)
{
    UNUSED(self);

    cmsx_WritebackPidFromArray(cmsx_pidRoll, PID_ROLL);
    cmsx_WritebackPidFromArray(cmsx_pidPitch, PID_PITCH);
    cmsx_WritebackPidFromArray(cmsx_pidYaw, PID_YAW);

    schedulePidGainsUpdate();

    return 0;
}
Esempio n. 3
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;
    };
}