From 33eef46db38c74fa112a05076ca6da6d4543cc36 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 16 Feb 2016 11:37:23 +0100 Subject: [PATCH] PID3 mw23 implementation Finish PID3 implementation --- src/main/config/config.c | 2 +- src/main/flight/pid.c | 175 +++++++++++++++++++++++++++++++++++++-- src/main/flight/pid.h | 5 +- src/main/io/serial_cli.c | 3 +- src/main/io/serial_msp.c | 2 +- src/main/mw.c | 8 +- 6 files changed, 185 insertions(+), 10 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index b77c2c9de..ac20cfc6a 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -176,6 +176,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->I8[PIDVEL] = 45; pidProfile->D8[PIDVEL] = 1; + pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX; pidProfile->dterm_lpf_hz = 0; // filtering ON by default pidProfile->deltaMethod = DELTA_FROM_MEASUREMENT; pidProfile->airModeInsaneAcrobilityFactor = 0; @@ -719,7 +720,6 @@ void activateConfig(void) #ifdef TELEMETRY telemetryUseConfig(&masterConfig.telemetryConfig); #endif - currentProfile->pidProfile.pidController = constrain(currentProfile->pidProfile.pidController, 1, 2); // This should prevent UNUSED values. CF 1.11 support pidSetController(currentProfile->pidProfile.pidController); #ifdef GPS diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 372efe844..373a51f36 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -48,6 +48,7 @@ #include "config/runtime_config.h" +extern uint8_t motorCount; extern float dT; extern bool motorLimitReached; @@ -60,18 +61,29 @@ int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; #define DELTA_TOTAL_SAMPLES 3 // PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction -uint8_t PIDweight[3]; +uint8_t dynP8[3], dynI8[3], dynD8[3], PIDweight[3]; static int32_t errorGyroI[3], errorGyroILimit[3]; static float errorGyroIf[3], errorGyroIfLimit[3]; +static int32_t errorAngleI[2]; +static float errorAngleIf[2]; -static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, +static void pidMultiWiiRewrite(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 = pidRewrite; // which pid controller are we using, defaultMultiWii +pidControllerFuncPtr pid_controller = pidMultiWiiRewrite; // which pid controller are we using, defaultMultiWii + +void pidResetErrorAngle(void) +{ + errorAngleI[ROLL] = 0; + errorAngleI[PITCH] = 0; + + errorAngleIf[ROLL] = 0.0f; + errorAngleIf[PITCH] = 0.0f; +} void pidResetErrorGyro(void) { @@ -262,7 +274,157 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa } } -static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, +static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, + rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) +{ + UNUSED(rxConfig); + + int axis, deltaCount, prop = 0; + int32_t rc, error, errorAngle, delta, gyroError; + int32_t PTerm, ITerm, PTermACC, ITermACC, DTerm; + static int16_t lastErrorForDelta[2]; + static int32_t previousDelta[3][DELTA_TOTAL_SAMPLES]; + + if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) { + for (axis = 0; axis < 3; axis++) BiQuadNewLpf(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], targetLooptime); + deltaStateIsSet = true; + } + + 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; + + gyroError = gyroADC[axis] / 4; + + error = rc - gyroError; + errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp 16 bits is ok here + + if (ABS(gyroADC[axis]) > (640 * 4)) { + errorGyroI[axis] = 0; + } + + // Anti windup protection + if (IS_RC_MODE_ACTIVE(BOXAIRMODE) || IS_RC_MODE_ACTIVE(BOXACROPLUS)) { + errorGyroI[axis] = (int32_t) (errorGyroI[axis] * scaleItermToRcInput(axis)); + if (antiWindupProtection || motorLimitReached) { + errorGyroI[axis] = constrain(errorGyroI[axis], -errorGyroILimit[axis], errorGyroILimit[axis]); + } else { + errorGyroILimit[axis] = ABS(errorGyroI[axis]); + } + } + + 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)gyroError * dynP8[axis]) >> 6; // 32 bits is needed for calculation + + //-----calculate D-term based on the configured approach (delta from measurement or deltafromError) + if (pidProfile->deltaMethod == DELTA_FROM_ERROR) { + delta = error - lastErrorForDelta[axis]; + lastErrorForDelta[axis] = error; + } else { /* Delta from measurement */ + delta = -(gyroError - lastErrorForDelta[axis]); + lastErrorForDelta[axis] = gyroError; + } + + if (deltaStateIsSet) { + DTerm = lrintf(applyBiQuadFilter((float) delta, &deltaBiQuadState[axis])) * 3; // Keep same scaling as unfiltered delta + } else { + // Apply moving average + DTerm = 0; + for (deltaCount = DELTA_TOTAL_SAMPLES -1; deltaCount > 0; deltaCount--) previousDelta[axis][deltaCount] = previousDelta[axis][deltaCount-1]; + previousDelta[axis][0] = delta; + for (deltaCount = 0; deltaCount < DELTA_TOTAL_SAMPLES; deltaCount++) DTerm += previousDelta[axis][deltaCount]; + delta = DTerm; + } + + DTerm = ((int32_t)DTerm * dynD8[axis]) >> 5; // 32 bits is needed for calculation + + axisPID[axis] = PTerm + ITerm + DTerm; + + if (IS_RC_MODE_ACTIVE(BOXACROPLUS)) { + acroPlusApply(&acroPlusState[axis], axis, pidProfile); + axisPID[axis] = lrintf(acroPlusState[axis].factor + acroPlusState[axis].wowFactor * axisPID[axis]); + } + +#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, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) { UNUSED(rxConfig); @@ -411,10 +573,13 @@ void pidSetController(pidControllerType_e type) switch (type) { default: case PID_CONTROLLER_MWREWRITE: - pid_controller = pidRewrite; + pid_controller = pidMultiWiiRewrite; break; 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 308c894df..4dd95450c 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -37,7 +37,8 @@ typedef enum { } pidIndex_e; typedef enum { - PID_CONTROLLER_MWREWRITE = 1, + PID_CONTROLLER_MW23, + PID_CONTROLLER_MWREWRITE, PID_CONTROLLER_LUX_FLOAT, PID_COUNT } pidControllerType_e; @@ -66,6 +67,7 @@ typedef struct pidProfile_s { uint16_t airModeInsaneAcrobilityFactor; // Air mode acrobility factor float dterm_lpf_hz; // Delta Filter in hz uint8_t deltaMethod; // Alternative delta Calculation + uint16_t yaw_p_limit; #ifdef GTUNE uint8_t gtune_lolimP[3]; // [0..200] Lower limit of P during G tune @@ -86,5 +88,6 @@ extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; bool antiWindupProtection; void pidSetController(pidControllerType_e type); +void pidResetErrorAngle(void); void pidResetErrorGyro(void); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 917caf25f..cbe309bfb 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -350,7 +350,7 @@ static const char * const lookupTableBlackboxDevice[] = { static const char * const lookupTablePidController[] = { - "UNUSED", "MWREWRITE", "LUX" + "MW23", "MWREWRITE", "LUX" }; static const char * const lookupTableSerialRX[] = { @@ -626,6 +626,7 @@ const clivalue_t valueTable[] = { { "yaw_motor_direction", VAR_INT8 | MASTER_VALUE, &masterConfig.mixerConfig.yaw_motor_direction, .config.minmax = { -1, 1 } }, { "airmode_saturation_limit", VAR_UINT8 | MASTER_VALUE, &masterConfig.mixerConfig.airmode_saturation_limit, .config.minmax = { 0, 100 } }, { "yaw_jump_prevention_limit", VAR_UINT16 | MASTER_VALUE, &masterConfig.mixerConfig.yaw_jump_prevention_limit, .config.minmax = { YAW_JUMP_PREVENTION_LIMIT_LOW, YAW_JUMP_PREVENTION_LIMIT_HIGH } }, + { "yaw_p_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_p_limit, .config.minmax = { YAW_P_LIMIT_MIN, YAW_P_LIMIT_MAX } }, #ifdef USE_SERVOS { "tri_unarmed_servo", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mixerConfig.tri_unarmed_servo, .config.lookup = { TABLE_OFF_ON } }, { "servo_lowpass_freq", VAR_INT16 | MASTER_VALUE, &masterConfig.mixerConfig.servo_lowpass_freq, .config.minmax = { 10, 400} }, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index cb70ec557..9d621c853 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1283,7 +1283,7 @@ static bool processInCommand(void) setGyroSamplingSpeed(read16()); break; case MSP_SET_PID_CONTROLLER: - currentProfile->pidProfile.pidController = constrain(read8(), 1, 2); + currentProfile->pidProfile.pidController = read8(); pidSetController(currentProfile->pidProfile.pidController); break; case MSP_SET_PID: diff --git a/src/main/mw.c b/src/main/mw.c index f1beac76a..37a5e80e8 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -116,7 +116,7 @@ int16_t telemTemperature1; // gyro sensor temperature static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero extern uint32_t currentTime; -extern uint8_t PIDweight[3]; +extern uint8_t dynP8[3], dynI8[3], dynD8[3], PIDweight[3]; extern bool antiWindupProtection; static filterStatePt1_t filteredCycleTimeState; @@ -256,6 +256,10 @@ void annexCode(void) rcCommand[axis] = (lookupYawRC[tmp2] + (tmp - tmp2 * 100) * (lookupYawRC[tmp2 + 1] - lookupYawRC[tmp2]) / 100) * -masterConfig.yaw_control_direction; prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * ABS(tmp) / 500; } + // FIXME axis indexes into pids. use something like lookupPidIndex(rc_alias_e alias) to reduce coupling. + dynP8[axis] = (uint16_t)currentProfile->pidProfile.P8[axis] * prop1 / 100; + dynI8[axis] = (uint16_t)currentProfile->pidProfile.I8[axis] * prop1 / 100; + dynD8[axis] = (uint16_t)currentProfile->pidProfile.D8[axis] * prop1 / 100; // non coupled PID reduction scaler used in PID controller 1 and PID controller 2. YAW TPA disabled. 100 means 100% of the pids if (axis == YAW) { @@ -292,6 +296,7 @@ void annexCode(void) scaleRcCommandToFpvCamAngle(); } + if (ARMING_FLAG(ARMED)) { LED0_ON; } else { @@ -481,6 +486,7 @@ void processRx(void) } } else { pidResetErrorGyro(); + pidResetErrorAngle(); } } else { antiWindupProtection = false;