rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #1707 from betaflight/pid_config_init 

Add PID config initialisation
This commit is contained in:
Martin Budden 2016-11-30 23:10:25 +01:00 committed by GitHub
parent 16eed3c27c 9a3c8191af
commit b19e439361
8 changed files with 57 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/main/cms/cms_menu_imu.c
View File

@ -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(&currentProfile->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(&currentProfile->pidProfile);
masterConfig.profile[profileIndex].pidProfile.P8[PIDLEVEL] = cmsx_angleStrength;
masterConfig.profile[profileIndex].pidProfile.I8[PIDLEVEL] = cmsx_horizonStrength;

2
src/main/fc/fc_msp.c
View File

@ -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(&currentProfile->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(&currentProfile->pidProfile);
break;
case MSP_SET_SENSOR_CONFIG:

23
src/main/fc/rc_controls.c
View File

@ -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 }}
}
};
@ -579,7 +589,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
@ -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;
};
@ -676,7 +694,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;

2
src/main/fc/rc_controls.h
View File

@ -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;

65
src/main/flight/pid.c
View File

@ -54,17 +54,20 @@ 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;
void pidSetTargetLooptime(uint32_t pidLooptime)
{
targetPidLooptime = pidLooptime;
dT = (float)targetPidLooptime * 0.000001f;
}
void pidResetErrorGyroState(void)
{
for (int axis = 0; axis < 3; axis++) {
errorGyroIf[axis] = 0.0f;
previousGyroIf[axis] = 0.0f;
}
}
@ -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 previousRateError[2];
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) {
@ -221,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,16 +251,15 @@ 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;
if (axis != FD_YAW) {
static float previousSetpoint[3];
float dynC = c[axis];
if (pidProfile->setpointRelaxRatio < 100) {
dynC = c[axis];
@ -282,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);
@ -298,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;

1
src/main/flight/pid.h
View File

@ -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);

1
src/main/main.c
View File

@ -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(&currentProfile->pidProfile);
pidInitConfig(&currentProfile->pidProfile);
imuInit();

1
src/main/target/COLIBRI_RACE/i2c_bst.c
View File

@ -708,6 +708,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
bstWrite8(currentProfile->pidProfile.I8[i]);
bstWrite8(currentProfile->pidProfile.D8[i]);
}
pidInitConfig(&currentProfile->pidProfile);
break;
case BST_PIDNAMES:
bstWriteNames(pidnames);