From 7f69537f86e1fec4c7d2557b71fe60f7b0af40bf Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Fri, 8 May 2015 15:20:53 +0200 Subject: [PATCH] PID1 TPA Implementation --- src/main/flight/pid.c | 8 ++++---- src/main/mw.c | 7 ++++++- 2 files changed, 10 insertions(+), 5 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 74ad06dc2a..06d25fa7a0 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -55,7 +55,7 @@ int16_t axisPID[3]; int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; #endif -uint8_t dynP8[3], dynI8[3], dynD8[3]; +uint8_t dynP8[3], dynI8[3], dynD8[3], redP8[3], redI8[3], redD8[3]; static int32_t errorGyroI[3] = { 0, 0, 0 }; static float errorGyroIf[3] = { 0.0f, 0.0f, 0.0f }; @@ -700,13 +700,13 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat RateError = AngleRateTmp - (gyroData[axis] / 4); // -----calculate P component - PTerm = (RateError * pidProfile->P8[axis]) >> 7; + PTerm = (RateError * redP8[axis]) >> 7; // -----calculate I component // there should be no division before accumulating the error to integrator, because the precision would be reduced. // Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used. // Time correction (to avoid different I scaling for different builds based on average cycle time) // is normalized to cycle time = 2048. - errorGyroI[axis] = errorGyroI[axis] + ((RateError * cycleTime) >> 11) * pidProfile->I8[axis]; + errorGyroI[axis] = errorGyroI[axis] + ((RateError * cycleTime) >> 11) * redI8[axis]; // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated. // I coefficient (I8) moved before integration to make limiting independent from PID settings @@ -724,7 +724,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat deltaSum = delta1[axis] + delta2[axis] + delta; delta2[axis] = delta1[axis]; delta1[axis] = delta; - DTerm = (deltaSum * pidProfile->D8[axis]) >> 8; + DTerm = (deltaSum * redD8[axis]) >> 8; // -----calculate total PID output axisPID[axis] = PTerm + ITerm + DTerm; diff --git a/src/main/mw.c b/src/main/mw.c index e00de99408..c102bbf968 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -100,7 +100,7 @@ int16_t headFreeModeHold; 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 uint8_t dynP8[3], dynI8[3], dynD8[3]; +extern uint8_t dynP8[3], dynI8[3], dynD8[3], redP8[3], redI8[3], redD8[3]; typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig); // pid controller function prototype @@ -218,6 +218,11 @@ void annexCode(void) dynI8[axis] = (uint16_t)currentProfile->pidProfile.I8[axis] * prop1 / 100; dynD8[axis] = (uint16_t)currentProfile->pidProfile.D8[axis] * prop1 / 100; + // non coupled PID reduction used in PID controller 1 + redP8[axis] = (uint16_t)currentProfile->pidProfile.P8[axis] * prop2 / 100; + redI8[axis] = (uint16_t)currentProfile->pidProfile.I8[axis] * prop2 / 100; + redD8[axis] = (uint16_t)currentProfile->pidProfile.D8[axis] * prop2 / 100; + if (rcData[axis] < masterConfig.rxConfig.midrc) rcCommand[axis] = -rcCommand[axis]; }