diff --git a/src/main/config/config.c b/src/main/config/config.c index 9baa7bca5f..5c7e86d9ac 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -174,7 +174,6 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->I8[PIDVEL] = 45; pidProfile->D8[PIDVEL] = 1; - pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX; pidProfile->dterm_cut_hz = 40; pidProfile->P_f[ROLL] = 1.5f; // new PID with preliminary defaults test carefully @@ -190,8 +189,6 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->H_level = 3.0f; pidProfile->H_sensitivity = 75; - pidProfile->pid5_oldyw = 0; - #ifdef GTUNE pidProfile->gtune_lolimP[ROLL] = 10; // [0..200] Lower limit of ROLL P during G tune. pidProfile->gtune_lolimP[PITCH] = 10; // [0..200] Lower limit of PITCH P during G tune. diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 44f2c0b06a..24616c95c5 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -65,13 +65,13 @@ static float errorGyroIf[3] = { 0.0f, 0.0f, 0.0f }; static int32_t errorAngleI[2] = { 0, 0 }; static float errorAngleIf[2] = { 0.0f, 0.0f }; -static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, +static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig); typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig); // pid controller function prototype -pidControllerFuncPtr pid_controller = pidMultiWii; // which pid controller are we using, defaultMultiWii +pidControllerFuncPtr pid_controller = pidRewrite; // which pid controller are we using, defaultMultiWii void pidResetErrorAngle(void) { @@ -216,472 +216,6 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa } } -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 }; - 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) - attitude.raw[axis] + angleTrim->raw[axis]; -#else - errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination), - +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; -#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 - - if (axis == YAW) { - PTerm = filterApplyPt1(PTerm, &yawPTermState, YAW_PTERM_FILTER, dT); - } - - delta = (gyroADC[axis] - lastGyro[axis]) / 4; - lastGyro[axis] = gyroADC[axis]; - - // Dterm low pass - if (pidProfile->dterm_cut_hz) { - delta = filterApplyPt1(delta, &DTermState[axis], pidProfile->dterm_cut_hz, dT); - } - - DTerm = (delta * 3 * dynD8[axis]) / 32; // Multiplied by 3 to match old scaling - axisPID[axis] = PTerm + ITerm - DTerm; - -#ifdef GTUNE - if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { - calculate_Gtune(axis); - } -#endif - -#ifdef BLACKBOX - axisPID_P[axis] = PTerm; - axisPID_I[axis] = ITerm; - axisPID_D[axis] = -DTerm; -#endif - } -} - -static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, - rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) -{ - UNUSED(rxConfig); - - int axis, prop = 0; - int32_t rc, error, errorAngle; - int32_t PTerm, ITerm, PTermACC, ITermACC, DTerm; - static int16_t lastGyro[2] = { 0, 0 }; - int32_t delta; - - if (FLIGHT_MODE(HORIZON_MODE)) { - prop = MIN(MAX(ABS(rcCommand[PITCH]), ABS(rcCommand[ROLL])), 512); - } - - // PITCH & ROLL - for (axis = 0; axis < 2; axis++) { - - rc = rcCommand[axis] << 1; - - error = rc - (gyroADC[axis] / 4); - errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp 16 bits is ok here - - if (ABS(gyroADC[axis]) > (640 * 4)) { - errorGyroI[axis] = 0; - } - - ITerm = (errorGyroI[axis] >> 7) * pidProfile->I8[axis] >> 6; // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000 - - PTerm = (int32_t)rc * pidProfile->P8[axis] >> 6; - - if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { // axis relying on ACC - // 50 degrees max inclination -#ifdef GPS - errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), - +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; -#else - errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination), - +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; -#endif - - errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); // WindUp //16 bits is ok here - - PTermACC = ((int32_t)errorAngle * pidProfile->P8[PIDLEVEL]) >> 7; // 32 bits is needed for calculation: errorAngle*P8 could exceed 32768 16 bits is ok for result - - int16_t limit = pidProfile->D8[PIDLEVEL] * 5; - PTermACC = constrain(PTermACC, -limit, +limit); - - ITermACC = ((int32_t)errorAngleI[axis] * pidProfile->I8[PIDLEVEL]) >> 12; // 32 bits is needed for calculation:10000*I8 could exceed 32768 16 bits is ok for result - - ITerm = ITermACC + ((ITerm - ITermACC) * prop >> 9); - PTerm = PTermACC + ((PTerm - PTermACC) * prop >> 9); - } - - PTerm -= ((int32_t)(gyroADC[axis] / 4) * dynP8[axis]) >> 6; // 32 bits is needed for calculation - - if (axis == YAW) { - PTerm = filterApplyPt1(PTerm, &yawPTermState, YAW_PTERM_FILTER, dT); - } - - delta = (gyroADC[axis] - lastGyro[axis]) / 4; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 - lastGyro[axis] = gyroADC[axis]; - - // Dterm low pass - if (pidProfile->dterm_cut_hz) { - delta = filterApplyPt1(delta, &DTermState[axis], pidProfile->dterm_cut_hz, dT); - } - - DTerm = ((int32_t)delta * 3 * dynD8[axis]) >> 5; // 32 bits is needed for calculation. Multiplied by 3 to match old scaling - - axisPID[axis] = PTerm + ITerm - DTerm; - -#ifdef GTUNE - if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { - calculate_Gtune(axis); - } -#endif - -#ifdef BLACKBOX - axisPID_P[axis] = PTerm; - axisPID_I[axis] = ITerm; - axisPID_D[axis] = -DTerm; -#endif - } - - //YAW - rc = (int32_t)rcCommand[FD_YAW] * (2 * controlRateConfig->rates[FD_YAW] + 30) >> 5; -#ifdef ALIENWII32 - error = rc - gyroADC[FD_YAW]; -#else - error = rc - (gyroADC[FD_YAW] / 4); -#endif - errorGyroI[FD_YAW] += (int32_t)error * pidProfile->I8[FD_YAW]; - errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW], 2 - ((int32_t)1 << 28), -2 + ((int32_t)1 << 28)); - if (ABS(rc) > 50) errorGyroI[FD_YAW] = 0; - - PTerm = (int32_t)error * pidProfile->P8[FD_YAW] >> 6; // TODO: Bitwise shift on a signed integer is not recommended - - // Constrain YAW by D value if not servo driven in that case servolimits apply - if(motorCount >= 4 && pidProfile->yaw_p_limit < YAW_P_LIMIT_MAX) { - PTerm = constrain(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit); - } - - ITerm = constrain((int16_t)(errorGyroI[FD_YAW] >> 13), -GYRO_I_MAX, +GYRO_I_MAX); - - axisPID[FD_YAW] = PTerm + ITerm; - -#ifdef GTUNE - if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { - calculate_Gtune(FD_YAW); - } -#endif - -#ifdef BLACKBOX - axisPID_P[FD_YAW] = PTerm; - axisPID_I[FD_YAW] = ITerm; - axisPID_D[FD_YAW] = 0; -#endif -} - -static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, - uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) -{ - UNUSED(rxConfig); - - int axis, prop; - int32_t rc, error, errorAngle; - int32_t PTerm, ITerm, PTermACC = 0, ITermACC = 0, PTermGYRO = 0, ITermGYRO = 0, DTerm; - static int16_t lastGyro[2] = { 0, 0 }; - int32_t delta; - - UNUSED(controlRateConfig); - - // **** PITCH & ROLL **** - prop = MIN(MAX(ABS(rcCommand[PITCH]), ABS(rcCommand[ROLL])), 500); // range [0;500] - - for (axis = 0; axis < 2; axis++) { - if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { // MODE relying on ACC - // observe max inclination -#ifdef GPS - errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), - +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; -#else - errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination), - +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; -#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)) { // MODE relying on GYRO - 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)) - errorGyroI[axis] = 0; - - ITermGYRO = (errorGyroI[axis] / 125 * pidProfile->I8[axis]) / 64; - } - if (FLIGHT_MODE(HORIZON_MODE)) { - PTerm = (PTermACC * (500 - prop) + PTermGYRO * prop) / 500; - ITerm = (ITermACC * (500 - prop) + ITermGYRO * prop) / 500; - } else { - if (FLIGHT_MODE(ANGLE_MODE)) { - 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 - - if (axis == YAW) { - PTerm = filterApplyPt1(PTerm, &yawPTermState, YAW_PTERM_FILTER, dT); - } - - delta = (gyroADC[axis] - lastGyro[axis]) / 4; - lastGyro[axis] = gyroADC[axis]; - - // Dterm low pass - if (pidProfile->dterm_cut_hz) { - delta = filterApplyPt1(delta, &DTermState[axis], pidProfile->dterm_cut_hz, dT); - } - - DTerm = (delta * 3 * dynD8[axis]) / 32; // Multiplied by 3 to match old scaling - axisPID[axis] = PTerm + ITerm - DTerm; - -#ifdef GTUNE - if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { - calculate_Gtune(axis); - } -#endif - -#ifdef BLACKBOX - axisPID_P[axis] = PTerm; - axisPID_I[axis] = ITerm; - axisPID_D[axis] = -DTerm; -#endif - } - //YAW - rc = (int32_t)rcCommand[FD_YAW] * (2 * controlRateConfig->rates[FD_YAW] + 30) >> 5; -#ifdef ALIENWII32 - error = rc - gyroADC[FD_YAW]; -#else - error = rc - (gyroADC[FD_YAW] / 4); -#endif - errorGyroI[FD_YAW] += (int32_t)error * pidProfile->I8[FD_YAW]; - errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW], 2 - ((int32_t)1 << 28), -2 + ((int32_t)1 << 28)); - if (ABS(rc) > 50) errorGyroI[FD_YAW] = 0; - - PTerm = (int32_t)error * pidProfile->P8[FD_YAW] >> 6; - - // Constrain YAW by D value if not servo driven in that case servolimits apply - if(motorCount >= 4 && pidProfile->yaw_p_limit < YAW_P_LIMIT_MAX) { - PTerm = constrain(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit); - } - - ITerm = constrain((int16_t)(errorGyroI[FD_YAW] >> 13), -GYRO_I_MAX, +GYRO_I_MAX); - - axisPID[FD_YAW] = PTerm + ITerm; - -#ifdef GTUNE - if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { - calculate_Gtune(FD_YAW); - } -#endif - -#ifdef BLACKBOX - axisPID_P[FD_YAW] = PTerm; - axisPID_I[FD_YAW] = ITerm; - axisPID_D[FD_YAW] = 0; -#endif -} - -static void pidHarakiri(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, -rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) -{ - UNUSED(rxConfig); - - float delta, RCfactor, rcCommandAxis, MainDptCut, gyroADCQuant; - float PTerm, ITerm, DTerm, PTermACC = 0.0f, ITermACC = 0.0f, ITermGYRO, error, prop = 0.0f; - static float lastGyro[2] = { 0.0f, 0.0f }, lastDTerm[2] = { 0.0f, 0.0f }; - uint8_t axis; - float ACCDeltaTimeINS, FLOATcycleTime, Mwii3msTimescale; - - if (pidProfile->dterm_cut_hz) { - MainDptCut = RCconstPI / constrain(pidProfile->dterm_cut_hz, 1, 50); // dterm_cut_hz (default 0, Range 1-50Hz) - } else { - MainDptCut = RCconstPI / 12.0f; // default is 12Hz to maintain initial behavior of PID5 - } - FLOATcycleTime = (float)constrain(cycleTime, 1, 100000); // 1us - 100ms - ACCDeltaTimeINS = FLOATcycleTime * 0.000001f; // ACCDeltaTimeINS is in seconds now - RCfactor = ACCDeltaTimeINS / (MainDptCut + ACCDeltaTimeINS); // used for pt1 element - - if (FLIGHT_MODE(HORIZON_MODE)) { - prop = (float)MIN(MAX(ABS(rcCommand[PITCH]), ABS(rcCommand[ROLL])), 450) / 450.0f; - } - - for (axis = 0; axis < 2; axis++) { - int32_t tmp = (int32_t)((float)gyroADC[axis] * 0.3125f); // Multiwii masks out the last 2 bits, this has the same idea - gyroADCQuant = (float)tmp * 3.2f; // but delivers more accuracy and also reduces jittery flight - rcCommandAxis = (float)rcCommand[axis]; // Calculate common values for pid controllers - if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { -#ifdef GPS - error = constrain(2.0f * rcCommandAxis + GPS_angle[axis], -((int) max_angle_inclination), +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; -#else - error = constrain(2.0f * rcCommandAxis, -((int) max_angle_inclination), +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; -#endif - - PTermACC = error * (float)pidProfile->P8[PIDLEVEL] * 0.008f; - float limitf = (float)pidProfile->D8[PIDLEVEL] * 5.0f; - PTermACC = constrain(PTermACC, -limitf, +limitf); - errorAngleIf[axis] = constrainf(errorAngleIf[axis] + error * ACCDeltaTimeINS, -30.0f, +30.0f); - ITermACC = errorAngleIf[axis] * (float)pidProfile->I8[PIDLEVEL] * 0.08f; - } - - if (!FLIGHT_MODE(ANGLE_MODE)) { - if (ABS((int16_t)gyroADC[axis]) > 2560) { - errorGyroIf[axis] = 0.0f; - } else { - error = (rcCommandAxis * 320.0f / (float)pidProfile->P8[axis]) - gyroADCQuant; - errorGyroIf[axis] = constrainf(errorGyroIf[axis] + error * ACCDeltaTimeINS, -192.0f, +192.0f); - } - - ITermGYRO = errorGyroIf[axis] * (float)pidProfile->I8[axis] * 0.01f; - - if (FLIGHT_MODE(HORIZON_MODE)) { - PTerm = PTermACC + prop * (rcCommandAxis - PTermACC); - ITerm = ITermACC + prop * (ITermGYRO - ITermACC); - } else { - PTerm = rcCommandAxis; - ITerm = ITermGYRO; - } - } else { - PTerm = PTermACC; - ITerm = ITermACC; - } - - PTerm -= gyroADCQuant * dynP8[axis] * 0.003f; - - delta = (gyroADCQuant - lastGyro[axis]) / ACCDeltaTimeINS; - - lastGyro[axis] = gyroADCQuant; - lastDTerm[axis] += RCfactor * (delta - lastDTerm[axis]); - DTerm = lastDTerm[axis] * dynD8[axis] * 0.00007f; - - axisPID[axis] = lrintf(PTerm + ITerm - DTerm); // Round up result. - -#ifdef GTUNE - if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { - calculate_Gtune(axis); - } -#endif - -#ifdef BLACKBOX - axisPID_P[axis] = PTerm; - axisPID_I[axis] = ITerm; - axisPID_D[axis] = -DTerm; -#endif - } - - Mwii3msTimescale = (int32_t)FLOATcycleTime & (int32_t)~3; // Filter last 2 bit jitter - Mwii3msTimescale /= 3000.0f; - - if (pidProfile->pid5_oldyw) { // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw - PTerm = ((int32_t)pidProfile->P8[FD_YAW] * (100 - (int32_t)controlRateConfig->rates[FD_YAW] * (int32_t)ABS(rcCommand[FD_YAW]) / 500)) / 100; - int32_t tmp = lrintf(gyroADC[FD_YAW] * 0.25f); - PTerm = rcCommand[FD_YAW] - tmp * PTerm / 80; - if ((ABS(tmp) > 640) || (ABS(rcCommand[FD_YAW]) > 100)) { - errorGyroI[FD_YAW] = 0; - } else { - error = ((int32_t)rcCommand[FD_YAW] * 80 / (int32_t)pidProfile->P8[FD_YAW]) - tmp; - errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW] + (int32_t)(error * Mwii3msTimescale), -16000, +16000); // WindUp - ITerm = (errorGyroI[FD_YAW] / 125 * pidProfile->I8[FD_YAW]) >> 6; - } - } else { - int32_t tmp = ((int32_t)rcCommand[FD_YAW] * (((int32_t)controlRateConfig->rates[FD_YAW] << 1) + 40)) >> 5; - error = tmp - lrintf(gyroADC[FD_YAW] * 0.25f); // Less Gyrojitter works actually better - - if (ABS(tmp) > 50) { - errorGyroI[FD_YAW] = 0; - } else { - errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW] + (int32_t)(error * (float)pidProfile->I8[FD_YAW] * Mwii3msTimescale), -268435454, +268435454); - } - - ITerm = constrain(errorGyroI[FD_YAW] >> 13, -GYRO_I_MAX, +GYRO_I_MAX); - PTerm = ((int32_t)error * (int32_t)pidProfile->P8[FD_YAW]) >> 6; - - if(motorCount >= 4 && pidProfile->yaw_p_limit < YAW_P_LIMIT_MAX) { // Constrain FD_YAW by D value if not servo driven in that case servolimits apply - PTerm = constrain(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit); - } - } - - axisPID[FD_YAW] = PTerm + ITerm; - axisPID[FD_YAW] = lrintf(axisPID[FD_YAW]); // Round up result. - -#ifdef GTUNE - if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { - calculate_Gtune(FD_YAW); - } -#endif - -#ifdef BLACKBOX - axisPID_P[FD_YAW] = PTerm; - axisPID_I[FD_YAW] = ITerm; - axisPID_D[FD_YAW] = 0; -#endif -} - static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) { @@ -806,24 +340,12 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat void pidSetController(pidControllerType_e type) { switch (type) { - case PID_CONTROLLER_MULTI_WII: default: - pid_controller = pidMultiWii; - break; case PID_CONTROLLER_REWRITE: pid_controller = pidRewrite; break; case PID_CONTROLLER_LUX_FLOAT: pid_controller = pidLuxFloat; - break; - case PID_CONTROLLER_MULTI_WII_23: - pid_controller = pidMultiWii23; - break; - case PID_CONTROLLER_MULTI_WII_HYBRID: - pid_controller = pidMultiWiiHybrid; - break; - case PID_CONTROLLER_HARAKIRI: - pid_controller = pidHarakiri; } } diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index c582472fa6..c3ca0c7a66 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -18,10 +18,6 @@ #pragma once #define GYRO_I_MAX 256 // Gyro I limiter -#define RCconstPI 0.159154943092f // 0.5f / M_PI; -#define OLD_YAW 0 // [0/1] 0 = MultiWii 2.3 yaw, 1 = older yaw. -#define YAW_P_LIMIT_MIN 100 // Maximum value for yaw P limiter -#define YAW_P_LIMIT_MAX 500 // Maximum value for yaw P limiter typedef enum { PIDROLL, @@ -38,18 +34,15 @@ typedef enum { } pidIndex_e; typedef enum { - PID_CONTROLLER_MULTI_WII, - PID_CONTROLLER_REWRITE, + PID_CONTROLLER_REWRITE = 1, PID_CONTROLLER_LUX_FLOAT, - PID_CONTROLLER_MULTI_WII_23, - PID_CONTROLLER_MULTI_WII_HYBRID, - PID_CONTROLLER_HARAKIRI, + PID_COUNT } pidControllerType_e; #define IS_PID_CONTROLLER_FP_BASED(pidController) (pidController == 2) typedef struct pidProfile_s { - uint8_t pidController; // 0 = multiwii original, 1 = rewrite from http://www.multiwii.com/forum/viewtopic.php?f=8&t=3671, 1, 2 = Luggi09s new baseflight pid + uint8_t pidController; // 1 = rewrite, 2 = luxfloat uint8_t P8[PID_ITEM_COUNT]; uint8_t I8[PID_ITEM_COUNT]; @@ -62,12 +55,7 @@ typedef struct pidProfile_s { float H_level; uint8_t H_sensitivity; - uint16_t yaw_p_limit; // set P term limit (fixed value was 300) uint8_t dterm_cut_hz; // (default 17Hz, Range 1-50Hz) Used for PT1 element in PID1, PID2 and PID5 - uint8_t pterm_cut_hz; // Used for fitlering Pterm noise on noisy frames - uint8_t gyro_cut_hz; // Used for soft gyro filtering - - uint8_t pid5_oldyw; // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw #ifdef GTUNE uint8_t gtune_lolimP[3]; // [0..200] Lower limit of P during G tune diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 7a2153b7b7..131aa6671d 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -186,6 +186,11 @@ static const rxFailsafeChannelMode_e rxFailsafeModesTable[RX_FAILSAFE_TYPE_COUNT { RX_FAILSAFE_MODE_INVALID, RX_FAILSAFE_MODE_HOLD, RX_FAILSAFE_MODE_SET } }; +// sync this with pidControllerType_e +static const char * const pidControllers[] = { + "REWRITE", "LUXFLOAT", NULL +}; + #ifndef CJMCU // sync this with sensors_e static const char * const sensorTypeNames[] = { @@ -476,7 +481,7 @@ const clivalue_t valueTable[] = { { "mag_hardware", VAR_UINT8 | MASTER_VALUE, &masterConfig.mag_hardware, 0, MAG_MAX }, { "mag_declination", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].mag_declination, -18000, 18000 }, - { "pid_controller", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidController, 0, 5 }, + { "pid_controller", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidController, 1, 2 }, { "p_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PITCH], 0, 200 }, { "i_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PITCH], 0, 200 }, @@ -514,11 +519,8 @@ const clivalue_t valueTable[] = { { "i_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDVEL], 0, 200 }, { "d_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDVEL], 0, 200 }, - { "yaw_p_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_p_limit, YAW_P_LIMIT_MIN, YAW_P_LIMIT_MAX }, { "dterm_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_cut_hz, 0, 200 }, - { "pid5_oldyw", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pid5_oldyw, 0, 1 }, - #ifdef GTUNE { "gtune_loP_rll", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_lolimP[FD_ROLL], 10, 200 }, { "gtune_loP_ptch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_lolimP[FD_PITCH], 10, 200 }, @@ -1409,7 +1411,13 @@ static void dumpValues(uint16_t mask) } printf("set %s = ", valueTable[i].name); - cliPrintVar(value, 0); + + if (strstr(valueTable[i].name, "pid_controller")) { + cliPrint(pidControllers[*(uint8_t *)valueTable[i].ptr -1]); + } else { + cliPrintVar(value, 0); + } + cliPrint("\r\n"); } } @@ -2049,9 +2057,42 @@ static void cliSet(char *cmdline) valuef = fastA2F(eqptr); for (i = 0; i < VALUE_COUNT; i++) { val = &valueTable[i]; - // ensure exact match when setting to prevent setting variables with shorter names if (strncasecmp(cmdline, valueTable[i].name, strlen(valueTable[i].name)) == 0 && variableNameLength == strlen(valueTable[i].name)) { - if (valuef >= valueTable[i].min && valuef <= valueTable[i].max) { // here we compare the float value since... it should work, RIGHT? + // ensure exact match when setting to prevent setting variables with shorter names + if (strstr(valueTable[i].name, "pid_controller")) { + int_float_value_t tmp; + tmp.int_value = 0; + bool pidControllerSet = false; + + if (value) { + if (value >= valueTable[i].min && value <= valueTable[i].max) { + tmp.int_value = value; + cliSetVar(val, tmp); + printf("%s set to %s \r\n", valueTable[i].name, pidControllers[value - 1]); + pidControllerSet = true; + } + } else { + for (int pid = 0; pid < (PID_COUNT - 1); pid++) { + if (strstr(eqptr, pidControllers[pid])) { + tmp.int_value = pid + 1; + cliSetVar(val, tmp); + printf("%s set to %s \r\n", valueTable[i].name, pidControllers[pid]); + pidControllerSet = true; + } + } + } + + if (!pidControllerSet) { + printf("Invalid Value! (Available PID Controllers: "); + for (int pid = 0; pid < (PID_COUNT - 1); pid++) { + printf(pidControllers[pid]); + printf(" "); + } + printf(")\r\n"); + } + + return; + } else if (valuef >= valueTable[i].min && valuef <= valueTable[i].max) { // here we compare the float value since... it should work, RIGHT? int_float_value_t tmp; if (valueTable[i].type & VAR_FLOAT) tmp.float_value = valuef; @@ -2073,6 +2114,7 @@ static void cliSet(char *cmdline) } } + static void cliGet(char *cmdline) { uint32_t i; @@ -2083,7 +2125,13 @@ static void cliGet(char *cmdline) if (strstr(valueTable[i].name, cmdline)) { val = &valueTable[i]; printf("%s = ", valueTable[i].name); - cliPrintVar(val, 0); + + if (strstr(valueTable[i].name, "pid_controller")) { + cliPrint(pidControllers[*(uint8_t *)valueTable[i].ptr - 1]); + } else { + cliPrintVar(val, 0); + } + cliPrint("\r\n"); matchedCommands++; diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 1ce2c000d0..5ae700039a 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1359,7 +1359,7 @@ static bool processInCommand(void) case MSP_SET_LOOP_TIME: break; case MSP_SET_PID_CONTROLLER: - currentProfile->pidProfile.pidController = read8(); + currentProfile->pidProfile.pidController = constrain(read8(), 1, 2); // Temporary configurator compatibility pidSetController(currentProfile->pidProfile.pidController); break; case MSP_SET_PID: