From 4a0f678dec1accc71651e84db9e8be430810f992 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 30 Nov 2016 12:52:43 +0100 Subject: [PATCH 1/5] Add PID config initialisation --- src/main/fc/fc_msp.c | 2 ++ src/main/flight/pid.c | 47 +++++++++++++++++-------------------------- src/main/flight/pid.h | 1 + src/main/main.c | 1 + 4 files changed, 23 insertions(+), 28 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index e6f999722c..2a1ac08004 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1264,6 +1264,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) currentProfile->pidProfile.I8[i] = sbufReadU8(src); currentProfile->pidProfile.D8[i] = sbufReadU8(src); } + pidInitConfig(¤tProfile->pidProfile); break; case MSP_SET_MODE_RANGE: @@ -1478,6 +1479,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) currentProfile->pidProfile.itermThrottleGain = sbufReadU8(src); currentProfile->pidProfile.rateAccelLimit = sbufReadU16(src); currentProfile->pidProfile.yawRateAccelLimit = sbufReadU16(src); + pidInitConfig(¤tProfile->pidProfile); break; case MSP_SET_SENSOR_CONFIG: diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 20839a2775..e3c893e3ca 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -56,9 +56,12 @@ int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; static float errorGyroIf[3]; +static float dT; + void pidSetTargetLooptime(uint32_t pidLooptime) { targetPidLooptime = pidLooptime; + dT = (float)targetPidLooptime * 0.000001f; } void pidResetErrorGyroState(void) @@ -91,7 +94,6 @@ void pidInitFilters(const pidProfile_t *pidProfile) static pt1Filter_t pt1FilterYaw; BUILD_BUG_ON(FD_YAW != 2); // only setting up Dterm filters on roll and pitch axes, so ensure yaw axis is 2 - const float dT = (float)targetPidLooptime * 0.000001f; if (pidProfile->dterm_notch_hz == 0) { dtermNotchFilterApplyFn = nullFilterApply; @@ -144,20 +146,28 @@ void pidInitFilters(const pidProfile_t *pidProfile) } } +static float Kp[3], Ki[3], Kd[3], c[3]; +static float rollPitchMaxVelocity, yawMaxVelocity, relaxFactor[3]; + +void pidInitConfig(const pidProfile_t *pidProfile) { + for(int axis = FD_ROLL; axis <= FD_YAW; axis++) { + Kp[axis] = PTERM_SCALE * pidProfile->P8[axis]; + Ki[axis] = ITERM_SCALE * pidProfile->I8[axis]; + Kd[axis] = DTERM_SCALE * pidProfile->D8[axis]; + c[axis] = pidProfile->dtermSetpointWeight / 100.0f; + relaxFactor[axis] = 1.0f - (pidProfile->setpointRelaxRatio / 100.0f); + } + yawMaxVelocity = pidProfile->yawRateAccelLimit * 1000 * dT; + rollPitchMaxVelocity = pidProfile->rateAccelLimit * 1000 * dT; +} + // Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage. // Based on 2DOF reference design (matlab) void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, uint16_t midrc) { static float lastRateError[2]; - static float Kp[3], Ki[3], Kd[3], c[3]; - static float rollPitchMaxVelocity, yawMaxVelocity; - static float previousSetpoint[3], relaxFactor[3]; - static float dT; - - if (!dT) { - dT = (float)targetPidLooptime * 0.000001f; - } + static float previousSetpoint[3]; float horizonLevelStrength = 1; if (FLIGHT_MODE(HORIZON_MODE)) { @@ -195,25 +205,6 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio // ----------PID controller---------- const float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { - - static uint8_t configP[3], configI[3], configD[3]; - - // Prevent unnecessary computing and check for changed PIDs. No need for individual checks. Only pids is fine for now - // Prepare all parameters for PID controller - if ((pidProfile->P8[axis] != configP[axis]) || (pidProfile->I8[axis] != configI[axis]) || (pidProfile->D8[axis] != configD[axis])) { - Kp[axis] = PTERM_SCALE * pidProfile->P8[axis]; - Ki[axis] = ITERM_SCALE * pidProfile->I8[axis]; - Kd[axis] = DTERM_SCALE * pidProfile->D8[axis]; - c[axis] = pidProfile->dtermSetpointWeight / 100.0f; - relaxFactor[axis] = 1.0f - (pidProfile->setpointRelaxRatio / 100.0f); - yawMaxVelocity = pidProfile->yawRateAccelLimit * 1000 * dT; - rollPitchMaxVelocity = pidProfile->rateAccelLimit * 1000 * dT; - - configP[axis] = pidProfile->P8[axis]; - configI[axis] = pidProfile->I8[axis]; - configD[axis] = pidProfile->D8[axis]; - } - // Limit abrupt yaw inputs / stops const float maxVelocity = (axis == FD_YAW) ? yawMaxVelocity : rollPitchMaxVelocity; if (maxVelocity) { diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index e059ed84c7..8ecd833e09 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -106,4 +106,5 @@ void pidResetErrorGyroState(void); void pidStabilisationState(pidStabilisationState_e pidControllerState); void pidSetTargetLooptime(uint32_t pidLooptime); void pidInitFilters(const pidProfile_t *pidProfile); +void pidInitConfig(const pidProfile_t *pidProfile); diff --git a/src/main/main.c b/src/main/main.c index 48849f012d..4aff1d44df 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -451,6 +451,7 @@ void init(void) // gyro.targetLooptime set in sensorsAutodetect(), so we are ready to call pidSetTargetLooptime() pidSetTargetLooptime((gyro.targetLooptime + LOOPTIME_SUSPEND_TIME) * masterConfig.pid_process_denom); // Initialize pid looptime pidInitFilters(¤tProfile->pidProfile); + pidInitConfig(¤tProfile->pidProfile); imuInit(); From 29690b9c0078602b3db281c57b6b38e6762f1655 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 30 Nov 2016 13:59:30 +0100 Subject: [PATCH 2/5] Add config reload to inflight adjustments and osd --- src/main/cms/cms_menu_imu.c | 2 ++ src/main/fc/rc_controls.c | 5 +++-- src/main/target/COLIBRI_RACE/i2c_bst.c | 1 + 3 files changed, 6 insertions(+), 2 deletions(-) diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 57ecc67ba9..3ad2818c3c 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -130,6 +130,7 @@ static long cmsx_PidWriteback(const OSD_Entry *self) masterConfig.profile[profileIndex].pidProfile.I8[i] = tempPid[i][1]; masterConfig.profile[profileIndex].pidProfile.D8[i] = tempPid[i][2]; } + pidInitConfig(¤tProfile->pidProfile); return 0; } @@ -248,6 +249,7 @@ static long cmsx_profileOtherOnExit(const OSD_Entry *self) masterConfig.profile[profileIndex].pidProfile.dtermSetpointWeight = cmsx_dtermSetpointWeight; masterConfig.profile[profileIndex].pidProfile.setpointRelaxRatio = cmsx_setpointRelaxRatio; + pidInitConfig(¤tProfile->pidProfile); masterConfig.profile[profileIndex].pidProfile.P8[PIDLEVEL] = cmsx_angleStrength; masterConfig.profile[profileIndex].pidProfile.I8[PIDLEVEL] = cmsx_horizonStrength; diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 96061c5b51..0e95b485c1 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -579,7 +579,7 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t case ADJUSTMENT_ROLL_D: newValue = constrain((int)pidProfile->D8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c pidProfile->D8[PIDROLL] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_D, newValue); + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_D, newValue); break; case ADJUSTMENT_YAW_P: newValue = constrain((int)pidProfile->P8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c @@ -676,7 +676,8 @@ void processRcAdjustments(controlRateConfig_t *controlRateConfig, rxConfig_t *rx continue; } - applyStepAdjustment(controlRateConfig, adjustmentFunction, delta); + applyStepAdjustment(controlRateConfig,adjustmentFunction,delta); + pidInitConfig(pidProfile); } else if (adjustmentState->config->mode == ADJUSTMENT_MODE_SELECT) { uint16_t rangeWidth = ((2100 - 900) / adjustmentState->config->data.selectConfig.switchPositions); uint8_t position = (constrain(rcData[channelIndex], 900, 2100 - 1) - 900) / rangeWidth; diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index 5774973293..b4d99b2166 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/src/main/target/COLIBRI_RACE/i2c_bst.c @@ -708,6 +708,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) bstWrite8(currentProfile->pidProfile.I8[i]); bstWrite8(currentProfile->pidProfile.D8[i]); } + pidInitConfig(¤tProfile->pidProfile); break; case BST_PIDNAMES: bstWriteNames(pidnames); From cbc8efd16abea9369f317e66212605e39b81052a Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 30 Nov 2016 14:19:19 +0100 Subject: [PATCH 3/5] Add inflight adjustments for Dterm sliders --- src/main/fc/rc_controls.c | 18 ++++++++++++++++++ src/main/fc/rc_controls.h | 2 ++ 2 files changed, 20 insertions(+) diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 0e95b485c1..1c8e51fe25 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -471,6 +471,16 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU .adjustmentFunction = ADJUSTMENT_RC_RATE_YAW, .mode = ADJUSTMENT_MODE_STEP, .data = { .stepConfig = { .step = 1 }} + }, + { + .adjustmentFunction = ADJUSTMENT_D_SETPOINT, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, + { + .adjustmentFunction = ADJUSTMENT_D_SETPOINT_TRANSITION, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} } }; @@ -601,6 +611,14 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t controlRateConfig->rcYawRate8 = newValue; blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_RATE_YAW, newValue); break; + case ADJUSTMENT_D_SETPOINT: + newValue = constrain((int)pidProfile->dtermSetpointWeight + delta, 0, 254); // FIXME magic numbers repeated in serial_cli.c + pidProfile->dtermSetpointWeight = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT, newValue); + case ADJUSTMENT_D_SETPOINT_TRANSITION: + newValue = constrain((int)pidProfile->setpointRelaxRatio + delta, 0, 100); // FIXME magic numbers repeated in serial_cli.c + pidProfile->setpointRelaxRatio = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT_TRANSITION, newValue); default: break; }; diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index a69c321df8..7d50636d3b 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -205,6 +205,8 @@ typedef enum { ADJUSTMENT_ROLL_I, ADJUSTMENT_ROLL_D, ADJUSTMENT_RC_RATE_YAW, + ADJUSTMENT_D_SETPOINT, + ADJUSTMENT_D_SETPOINT_TRANSITION, ADJUSTMENT_FUNCTION_COUNT, } adjustmentFunction_e; From 28ce3081c64be93e94d731ba74f50e7a90194255 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 30 Nov 2016 21:26:58 +0100 Subject: [PATCH 4/5] Remove double define // name consistency --- src/main/flight/pid.c | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index e3c893e3ca..d699829ca0 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -166,7 +166,7 @@ void pidInitConfig(const pidProfile_t *pidProfile) { void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, uint16_t midrc) { - static float lastRateError[2]; + static float previousRateError[2]; static float previousSetpoint[3]; float horizonLevelStrength = 1; @@ -212,7 +212,6 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio if (ABS(currentVelocity) > maxVelocity) { setpointRate[axis] = (currentVelocity > 0) ? previousSetpoint[axis] + maxVelocity : previousSetpoint[axis] - maxVelocity; } - previousSetpoint[axis] = setpointRate[axis]; } // Yaw control is GYRO based, direct sticks control is applied to rate PID @@ -261,7 +260,6 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio // -----calculate D component (Yaw D not yet supported) float DTerm = 0.0; if (axis != FD_YAW) { - static float previousSetpoint[3]; float dynC = c[axis]; if (pidProfile->setpointRelaxRatio < 100) { dynC = c[axis]; @@ -273,11 +271,10 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio dynC = dynC * powerf(rcInput[axis], 2) * relaxFactor[axis] + dynC * (1-relaxFactor[axis]); } } - previousSetpoint[axis] = setpointRate[axis]; const float rD = dynC * setpointRate[axis] - PVRate; // cr - y // Divide rate change by dT to get differential (ie dr/dt) - const float delta = (rD - lastRateError[axis]) / dT; - lastRateError[axis] = rD; + const float delta = (rD - previousRateError[axis]) / dT; + previousRateError[axis] = rD; DTerm = Kd[axis] * delta * tpaFactor; DEBUG_SET(DEBUG_DTERM_FILTER, axis, DTerm); @@ -289,6 +286,7 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio } else { PTerm = ptermYawFilterApplyFn(ptermYawFilter, PTerm); } + previousSetpoint[axis] = setpointRate[axis]; // -----calculate total PID output axisPIDf[axis] = PTerm + ITerm + DTerm; From 9a3c8191af6e707079337f21b1619ae04f9b86d8 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 30 Nov 2016 21:32:28 +0100 Subject: [PATCH 5/5] Refactor errorGyroIf --- src/main/flight/pid.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index d699829ca0..2d40bb89c7 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -54,7 +54,7 @@ uint8_t PIDweight[3]; int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; #endif -static float errorGyroIf[3]; +static float previousGyroIf[3]; static float dT; @@ -67,7 +67,7 @@ void pidSetTargetLooptime(uint32_t pidLooptime) void pidResetErrorGyroState(void) { for (int axis = 0; axis < 3; axis++) { - errorGyroIf[axis] = 0.0f; + previousGyroIf[axis] = 0.0f; } } @@ -251,11 +251,11 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio const float setpointRateScaler = constrainf(1.0f - (ABS(setpointRate[axis]) / accumulationThreshold), 0.0f, 1.0f); const float itermScaler = setpointRateScaler * kiThrottleGain; - float ITerm = errorGyroIf[axis]; + float ITerm = previousGyroIf[axis]; ITerm += Ki[axis] * errorRate * dT * itermScaler;; // limit maximum integrator value to prevent WindUp ITerm = constrainf(ITerm, -250.0f, 250.0f); - errorGyroIf[axis] = ITerm; + previousGyroIf[axis] = ITerm; // -----calculate D component (Yaw D not yet supported) float DTerm = 0.0;