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]);
}
