diff --git a/src/main/config/config.c b/src/main/config/config.c index 280eded6af..373e026780 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -171,8 +171,8 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->P8[PIDNAVR] = 14; // FW_NAV_P * 100 pidProfile->I8[PIDNAVR] = 2; // FW_NAV_I * 100 pidProfile->D8[PIDNAVR] = 80; // FW_NAV_D * 1000 - pidProfile->P8[PIDLEVEL] = 90; - pidProfile->I8[PIDLEVEL] = 10; + pidProfile->P8[PIDLEVEL] = 20; + pidProfile->I8[PIDLEVEL] = 20; pidProfile->D8[PIDLEVEL] = 100; pidProfile->P8[PIDMAG] = 40; pidProfile->P8[PIDVEL] = 100; // NAV_VEL_Z_P * 100 @@ -760,6 +760,7 @@ void activateConfig(void) telemetryUseConfig(&masterConfig.telemetryConfig); #endif + currentProfile->pidProfile.pidController = constrain(currentProfile->pidProfile.pidController, 1, 2); // This should limit to USED values only pidSetController(currentProfile->pidProfile.pidController); useFailsafeConfig(&masterConfig.failsafeConfig); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 8198a2ff12..d2157dd575 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -215,126 +215,6 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa } } -static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, - 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 }; - static int32_t delta1[2] = { 0, 0 }, delta2[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 - errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination), - +max_angle_inclination) - attitude.raw[axis]; - - 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 - - // Pterm low pass - if (pidProfile->pterm_cut_hz) { - PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz, 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 = delta1[axis] + delta2[axis] + delta; - delta2[axis] = delta1[axis]; - delta1[axis] = delta; - - // Dterm low pass - if (pidProfile->dterm_cut_hz) { - DTerm = filterApplyPt1(DTerm, &DTermState[axis], pidProfile->dterm_cut_hz, dT); - } - - DTerm = ((int32_t)DTerm * dynD8[axis]) >> 5; // 32 bits is needed for calculation - - 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 pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rxConfig_t *rxConfig) { @@ -467,7 +347,5 @@ void pidSetController(pidControllerType_e type) case PID_CONTROLLER_LUX_FLOAT: pid_controller = pidLuxFloat; break; - case PID_CONTROLLER_MW23: - pid_controller = pidMultiWii23; } } diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 13efef6a54..4a468fa463 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -36,8 +36,7 @@ typedef enum { } pidIndex_e; typedef enum { - PID_CONTROLLER_MW23 = 0, - PID_CONTROLLER_MWREWRITE, + PID_CONTROLLER_MWREWRITE = 1, PID_CONTROLLER_LUX_FLOAT, PID_COUNT } pidControllerType_e; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 04f68b2210..ad3c55e06e 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -340,7 +340,7 @@ static const char * const lookupTableGimbalMode[] = { }; static const char * const lookupTablePidController[] = { - "MW23", "MWREWRITE", "LUX" + "UNUSED", "MWREWRITE", "LUX" }; static const char * const lookupTableBlackboxDevice[] = { diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 9adedd1ab2..4e38aebac9 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1338,7 +1338,7 @@ static bool processInCommand(void) masterConfig.looptime = read16(); break; case MSP_SET_PID_CONTROLLER: - currentProfile->pidProfile.pidController = constrain(read8(), 0, 2); // Temporary configurator compatibility + currentProfile->pidProfile.pidController = constrain(read8(), 1, 2); // Temporary configurator compatibility pidSetController(currentProfile->pidProfile.pidController); break; case MSP_SET_PID: