static void updatePositionAccelController_MC(uint32_t deltaMicros, float maxAccelLimit) { float velErrorX, velErrorY, newAccelX, newAccelY; // Calculate velocity error velErrorX = posControl.desiredState.vel.V.X - posControl.actualState.vel.V.X; velErrorY = posControl.desiredState.vel.V.Y - posControl.actualState.vel.V.Y; // Calculate XY-acceleration limit according to velocity error limit float accelLimitX, accelLimitY; float velErrorMagnitude = sqrtf(sq(velErrorX) + sq(velErrorY)); if (velErrorMagnitude > 0.1f) { accelLimitX = maxAccelLimit / velErrorMagnitude * fabsf(velErrorX); accelLimitY = maxAccelLimit / velErrorMagnitude * fabsf(velErrorY); } else { accelLimitX = maxAccelLimit / 1.414213f; accelLimitY = accelLimitX; } // Apply additional jerk limiting of 1700 cm/s^3 (~100 deg/s), almost any copter should be able to achieve this rate // This will assure that we wont't saturate out LEVEL and RATE PID controller float maxAccelChange = US2S(deltaMicros) * 1700.0f; float accelLimitXMin = constrainf(lastAccelTargetX - maxAccelChange, -accelLimitX, +accelLimitX); float accelLimitXMax = constrainf(lastAccelTargetX + maxAccelChange, -accelLimitX, +accelLimitX); float accelLimitYMin = constrainf(lastAccelTargetY - maxAccelChange, -accelLimitY, +accelLimitY); float accelLimitYMax = constrainf(lastAccelTargetY + maxAccelChange, -accelLimitY, +accelLimitY); // TODO: Verify if we need jerk limiting after all // Apply PID with output limiting and I-term anti-windup // Pre-calculated accelLimit and the logic of navPidApply2 function guarantee that our newAccel won't exceed maxAccelLimit // Thus we don't need to do anything else with calculated acceleration newAccelX = navPidApply2(posControl.desiredState.vel.V.X, posControl.actualState.vel.V.X, US2S(deltaMicros), &posControl.pids.vel[X], accelLimitXMin, accelLimitXMax, false); newAccelY = navPidApply2(posControl.desiredState.vel.V.Y, posControl.actualState.vel.V.Y, US2S(deltaMicros), &posControl.pids.vel[Y], accelLimitYMin, accelLimitYMax, false); // Save last acceleration target lastAccelTargetX = newAccelX; lastAccelTargetY = newAccelY; // Apply LPF to jerk limited acceleration target float accelN = filterApplyPt1(newAccelX, &mcPosControllerAccFilterStateX, NAV_ACCEL_CUTOFF_FREQUENCY_HZ, US2S(deltaMicros)); float accelE = filterApplyPt1(newAccelY, &mcPosControllerAccFilterStateY, NAV_ACCEL_CUTOFF_FREQUENCY_HZ, US2S(deltaMicros)); // Rotate acceleration target into forward-right frame (aircraft) float accelForward = accelN * posControl.actualState.cosYaw + accelE * posControl.actualState.sinYaw; float accelRight = -accelN * posControl.actualState.sinYaw + accelE * posControl.actualState.cosYaw; // Calculate banking angles float desiredPitch = atan2_approx(accelForward, GRAVITY_CMSS); float desiredRoll = atan2_approx(accelRight * cos_approx(desiredPitch), GRAVITY_CMSS); int16_t maxBankAngle = DEGREES_TO_DECIDEGREES(posControl.navConfig->mc_max_bank_angle); posControl.rcAdjustment[ROLL] = constrain(RADIANS_TO_DECIDEGREES(desiredRoll), -maxBankAngle, maxBankAngle); posControl.rcAdjustment[PITCH] = constrain(RADIANS_TO_DECIDEGREES(desiredPitch), -maxBankAngle, maxBankAngle); }
static void updateTargetAngle(void) { if (rising) { targetAngle = AUTOTUNE_TARGET_ANGLE; } else { targetAngle = -AUTOTUNE_TARGET_ANGLE; } #if 0 debug[2] = DEGREES_TO_DECIDEGREES(targetAngle); #endif }
static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) { UNUSED(rxConfig); int axis, prop; int32_t error, errorAngle; int32_t PTerm, ITerm, PTermACC = 0, ITermACC = 0, PTermGYRO = 0, ITermGYRO = 0, DTerm; static int16_t lastGyro[3] = { 0, 0, 0 }; static int32_t delta1[3], delta2[3]; int32_t deltaSum; int32_t delta; UNUSED(controlRateConfig); // **** PITCH & ROLL & YAW PID **** prop = MIN(MAX(ABS(rcCommand[PITCH]), ABS(rcCommand[ROLL])), 500); // range [0;500] for (axis = 0; axis < 3; axis++) { if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && (axis == FD_ROLL || axis == FD_PITCH)) { // MODE relying on ACC // observe max inclination #ifdef GPS errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; #else errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination), +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; #endif #ifdef AUTOTUNE if (shouldAutotune()) { errorAngle = DEGREES_TO_DECIDEGREES(autotune(rcAliasToAngleIndexMap[axis], &inclination, DECIDEGREES_TO_DEGREES(errorAngle))); } #endif PTermACC = errorAngle * pidProfile->P8[PIDLEVEL] / 100; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result PTermACC = constrain(PTermACC, -pidProfile->D8[PIDLEVEL] * 5, +pidProfile->D8[PIDLEVEL] * 5); errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); // WindUp ITermACC = (errorAngleI[axis] * pidProfile->I8[PIDLEVEL]) >> 12; } if (!FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || axis == FD_YAW) { // MODE relying on GYRO or YAW axis error = (int32_t) rcCommand[axis] * 10 * 8 / pidProfile->P8[axis]; error -= gyroADC[axis] / 4; PTermGYRO = rcCommand[axis]; errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp if ((ABS(gyroADC[axis]) > (640 * 4)) || (axis == FD_YAW && ABS(rcCommand[axis]) > 100)) errorGyroI[axis] = 0; ITermGYRO = (errorGyroI[axis] / 125 * pidProfile->I8[axis]) / 64; } if (FLIGHT_MODE(HORIZON_MODE) && (axis == FD_ROLL || axis == FD_PITCH)) { PTerm = (PTermACC * (500 - prop) + PTermGYRO * prop) / 500; ITerm = (ITermACC * (500 - prop) + ITermGYRO * prop) / 500; } else { if (FLIGHT_MODE(ANGLE_MODE) && (axis == FD_ROLL || axis == FD_PITCH)) { PTerm = PTermACC; ITerm = ITermACC; } else { PTerm = PTermGYRO; ITerm = ITermGYRO; } } PTerm -= ((int32_t)gyroADC[axis] / 4) * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation // Pterm low pass if (pidProfile->pterm_cut_hz) { PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz); } delta = (gyroADC[axis] - lastGyro[axis]) / 4; lastGyro[axis] = gyroADC[axis]; deltaSum = delta1[axis] + delta2[axis] + delta; delta2[axis] = delta1[axis]; delta1[axis] = delta; // Dterm low pass if (pidProfile->dterm_cut_hz) { deltaSum = filterApplyPt1(deltaSum, &DTermState[axis], pidProfile->dterm_cut_hz); } DTerm = (deltaSum * dynD8[axis]) / 32; axisPID[axis] = PTerm + ITerm - DTerm; #ifdef BLACKBOX axisPID_P[axis] = PTerm; axisPID_I[axis] = ITerm; axisPID_D[axis] = -DTerm; #endif }
void applyFixedWingLaunchController(timeUs_t currentTimeUs) { // Called at PID rate if (launchState.launchDetected) { bool applyLaunchIdleLogic = true; if (launchState.motorControlAllowed) { // If launch detected we are in launch procedure - control airplane const float timeElapsedSinceLaunchMs = US2MS(currentTimeUs - launchState.launchStartedTime); // If user moves the stick - finish the launch if ((ABS(rcCommand[ROLL]) > rcControlsConfig()->pos_hold_deadband) || (ABS(rcCommand[PITCH]) > rcControlsConfig()->pos_hold_deadband)) { launchState.launchFinished = true; } // Abort launch after a pre-set time if (timeElapsedSinceLaunchMs >= navConfig()->fw.launch_timeout) { launchState.launchFinished = true; } // Motor control enabled if (timeElapsedSinceLaunchMs >= navConfig()->fw.launch_motor_timer) { // Don't apply idle logic anymore applyLaunchIdleLogic = false; // Increase throttle gradually over `launch_motor_spinup_time` milliseconds if (navConfig()->fw.launch_motor_spinup_time > 0) { const float timeSinceMotorStartMs = constrainf(timeElapsedSinceLaunchMs - navConfig()->fw.launch_motor_timer, 0.0f, navConfig()->fw.launch_motor_spinup_time); const uint16_t minIdleThrottle = MAX(motorConfig()->minthrottle, navConfig()->fw.launch_idle_throttle); rcCommand[THROTTLE] = scaleRangef(timeSinceMotorStartMs, 0.0f, navConfig()->fw.launch_motor_spinup_time, minIdleThrottle, navConfig()->fw.launch_throttle); } else { rcCommand[THROTTLE] = navConfig()->fw.launch_throttle; } } } if (applyLaunchIdleLogic) { // Launch idle logic - low throttle and zero out PIDs applyFixedWingLaunchIdleLogic(); } } else { // We are waiting for launch - update launch detector updateFixedWingLaunchDetector(currentTimeUs); // Launch idle logic - low throttle and zero out PIDs applyFixedWingLaunchIdleLogic(); } // Control beeper if (!launchState.launchFinished) { beeper(BEEPER_LAUNCH_MODE_ENABLED); } // Lock out controls rcCommand[ROLL] = 0; rcCommand[PITCH] = pidAngleToRcCommand(-DEGREES_TO_DECIDEGREES(navConfig()->fw.launch_climb_angle), pidProfile()->max_angle_inclination[FD_PITCH]); rcCommand[YAW] = 0; }
float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclination, float errorAngle) { float currentAngle; if (!(phase == PHASE_TUNE_ROLL || phase == PHASE_TUNE_PITCH) || autoTuneAngleIndex != angleIndex) { return errorAngle; } if (pidController == 2) { // TODO support new baseflight pid controller return errorAngle; } debug[0] = 0; #if 0 debug[1] = inclination->rawAngles[angleIndex]; #endif updatePidIndex(); if (rising) { currentAngle = DECIDEGREES_TO_DEGREES(inclination->raw[angleIndex]); } else { targetAngle = -targetAngle; currentAngle = DECIDEGREES_TO_DEGREES(-inclination->raw[angleIndex]); } #if 1 debug[1] = DEGREES_TO_DECIDEGREES(currentAngle); debug[2] = DEGREES_TO_DECIDEGREES(targetAngle); #endif if (firstPeakAngle == 0) { // The peak will be when our angular velocity is negative. To be sure we are in the right place, // we also check to make sure our angle position is greater than zero. if (currentAngle > secondPeakAngle) { // we are still going up secondPeakAngle = currentAngle; targetAngleAtPeak = targetAngle; debug[3] = DEGREES_TO_DECIDEGREES(secondPeakAngle); } else if (secondPeakAngle > 0) { if (cycleCount == 0) { // when checking the I value, we would like to overshoot the target position by half of the max oscillation. if (currentAngle - targetAngle < AUTOTUNE_MAX_OSCILLATION_ANGLE / 2) { pid.i *= AUTOTUNE_INCREASE_MULTIPLIER; } else { pid.i *= AUTOTUNE_DECREASE_MULTIPLIER; if (pid.i < AUTOTUNE_MINIMUM_I_VALUE) { pid.i = AUTOTUNE_MINIMUM_I_VALUE; } } // go back to checking P and D cycleCount = 1; pidProfile->I8[pidIndex] = 0; startNewCycle(); } else { // we are checking P and D values // set up to look for the 2nd peak firstPeakAngle = currentAngle; timeoutAt = millis() + AUTOTUNE_SETTLING_DELAY_MS; } } } else { // we saw the first peak. looking for the second if (currentAngle < firstPeakAngle) { firstPeakAngle = currentAngle; debug[3] = DEGREES_TO_DECIDEGREES(firstPeakAngle); } float oscillationAmplitude = secondPeakAngle - firstPeakAngle; uint32_t now = millis(); int32_t signedDiff = now - timeoutAt; bool timedOut = signedDiff >= 0L; // stop looking for the 2nd peak if we time out or if we change direction again after moving by more than half the maximum oscillation if (timedOut || (oscillationAmplitude > AUTOTUNE_MAX_OSCILLATION_ANGLE / 2 && currentAngle > firstPeakAngle)) { // analyze the data // Our goal is to have zero overshoot and to have AUTOTUNEMAXOSCILLATION amplitude if (secondPeakAngle > targetAngleAtPeak) { // overshot debug[0] = 1; #ifdef PREFER_HIGH_GAIN_SOLUTION if (oscillationAmplitude > AUTOTUNE_MAX_OSCILLATION_ANGLE) { // we have too much oscillation, so we can't increase D, so decrease P pid.p *= AUTOTUNE_DECREASE_MULTIPLIER; } else { // we don't have too much oscillation, so we can increase D pid.d *= AUTOTUNE_INCREASE_MULTIPLIER; } #else pid.p *= AUTOTUNE_DECREASE_MULTIPLIER; pid.d *= AUTOTUNE_INCREASE_MULTIPLIER; #endif } else { // undershot debug[0] = 2; if (oscillationAmplitude > AUTOTUNE_MAX_OSCILLATION_ANGLE) { // we have too much oscillation pid.d *= AUTOTUNE_DECREASE_MULTIPLIER; } else { // we don't have too much oscillation pid.p *= AUTOTUNE_INCREASE_MULTIPLIER; } } pidProfile->P8[pidIndex] = pid.p * MULTIWII_P_MULTIPLIER; pidProfile->D8[pidIndex] = pid.d; // switch to the other direction and start a new cycle startNewCycle(); if (++cycleCount == 3) { // switch to testing I value cycleCount = 0; pidProfile->I8[pidIndex] = pid.i * MULTIWII_I_MULTIPLIER; } } } if (angleIndex == AI_ROLL) { debug[0] += 100; } updateTargetAngle(); return targetAngle - DECIDEGREES_TO_DEGREES(inclination->raw[angleIndex]); }