Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-15 20:35:33 +03:00
Code Issues Wiki Activity

Move PID relaxation to only D

Browse source
This commit is contained in:
borisbstyle 2016-09-27 00:18:31 +02:00
parent 3b4e854fca
commit 73e952ce16
8 changed files with 23 additions and 31 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/blackbox/blackbox.c
View file

@ -1280,7 +1280,7 @@ static bool blackboxWriteSysinfo()
// Betaflight PID controller parameters
BLACKBOX_PRINT_HEADER_LINE("itermThrottleGain:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.itermThrottleGain);
BLACKBOX_PRINT_HEADER_LINE("ptermSRateWeight:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.ptermSRateWeight);
BLACKBOX_PRINT_HEADER_LINE("setpointRelaxRatio:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.setpointRelaxRatio);
BLACKBOX_PRINT_HEADER_LINE("dtermSetpointWeight:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.dtermSetpointWeight);
BLACKBOX_PRINT_HEADER_LINE("yawRateAccelLimit:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.yawRateAccelLimit);
BLACKBOX_PRINT_HEADER_LINE("rateAccelLimit:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.rateAccelLimit);

4
src/main/config/config.c
View file

@ -247,8 +247,8 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->pidAtMinThrottle = PID_STABILISATION_ON;
// Betaflight PID controller parameters
pidProfile->ptermSRateWeight = 85;
pidProfile->dtermSetpointWeight = 150;
pidProfile->setpointRelaxRatio = 70;
pidProfile->dtermSetpointWeight = 200;
pidProfile->yawRateAccelLimit = 220;
pidProfile->rateAccelLimit = 0;
pidProfile->itermThrottleGain = 0;

22
src/main/flight/pid.c
View file

@ -49,7 +49,7 @@
extern uint8_t motorCount;
uint32_t targetPidLooptime;
extern float setpointRate[3], ptermSetpointRate[3];
extern float setpointRate[3];
extern float rcInput[3];
static bool pidStabilisationEnabled;
@ -132,10 +132,10 @@ void initFilters(const pidProfile_t *pidProfile) {
static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig)
{
float errorRate = 0, rP = 0, rD = 0, PVRate = 0;
float errorRate = 0, rD = 0, PVRate = 0, dynC;
float ITerm,PTerm,DTerm;
static float lastRateError[2];
static float Kp[3], Ki[3], Kd[3], c[3], rollPitchMaxVelocity, yawMaxVelocity, previousSetpoint[3];
static float Kp[3], Ki[3], Kd[3], c[3], rollPitchMaxVelocity, yawMaxVelocity, previousSetpoint[3], relaxFactor[3];
float delta;
int axis;
float horizonLevelStrength = 1;
@ -188,6 +188,7 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc
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 * getdT();
rollPitchMaxVelocity = pidProfile->rateAccelLimit * 1000 * getdT();
@ -218,11 +219,11 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc
#endif
if (FLIGHT_MODE(ANGLE_MODE)) {
// ANGLE mode - control is angle based, so control loop is needed
ptermSetpointRate[axis] = setpointRate[axis] = errorAngle * pidProfile->P8[PIDLEVEL] / 10.0f;
setpointRate[axis] = errorAngle * pidProfile->P8[PIDLEVEL] / 10.0f;
} else {
// HORIZON mode - direct sticks control is applied to rate PID
// mix up angle error to desired AngleRate to add a little auto-level feel
ptermSetpointRate[axis] = setpointRate[axis] = setpointRate[axis] + (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 10.0f);
setpointRate[axis] = setpointRate[axis] + (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 10.0f);
}
}
@ -233,10 +234,9 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc
// Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
// ----- calculate error / angle rates ----------
errorRate = setpointRate[axis] - PVRate; // r - y
rP = ptermSetpointRate[axis] - PVRate; // br - y
// -----calculate P component
PTerm = Kp[axis] * rP * tpaFactor;
// -----calculate P component and add Dynamic Part based on stick input
PTerm = Kp[axis] * errorRate * tpaFactor;
// -----calculate I component.
// Reduce strong Iterm accumulation during higher stick inputs
@ -254,7 +254,11 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc
//-----calculate D-term (Yaw D not yet supported)
if (axis != YAW) {
rD = c[axis] * setpointRate[axis] - PVRate; // cr - y
if (pidProfile->setpointRelaxRatio < 100)
dynC = c[axis] * powerf(rcInput[axis], 2) * relaxFactor[axis] + c[axis] * (1-relaxFactor[axis]);
else
dynC = c[axis];
rD = dynC * setpointRate[axis] - PVRate; // cr - y
delta = rD - lastRateError[axis];
lastRateError[axis] = rD;

2
src/main/flight/pid.h
View file

@ -90,7 +90,7 @@ typedef struct pidProfile_s {
// Betaflight PID controller parameters
uint8_t itermThrottleGain; // Throttle coupling to iterm. Quick throttle changes will bump iterm
uint8_t ptermSRateWeight; // Setpoint super expo ratio for Pterm (lower means that pretty much only P has super expo rates)
uint8_t setpointRelaxRatio; // Setpoint weight relaxation effect
uint8_t dtermSetpointWeight; // Setpoint weight for Dterm (0= measurement, 1= full error, 1 > agressive derivative)
uint16_t yawRateAccelLimit; // yaw accel limiter for deg/sec/ms
uint16_t rateAccelLimit; // accel limiter roll/pitch deg/sec/ms

2
src/main/io/osd.c
View file

@ -477,7 +477,7 @@ static OSD_FLOAT_t entryRcExpo = {&rateProfile.rcExpo8, 0, 100, 1, 10};
static OSD_FLOAT_t entryRcExpoYaw = {&rateProfile.rcYawExpo8, 0, 100, 1, 10};
static OSD_FLOAT_t extryTpaEntry = {&rateProfile.dynThrPID, 0, 70, 1, 10};
static OSD_UINT16_t entryTpaBreak = {&rateProfile.tpa_breakpoint, 1100, 1800, 10};
static OSD_FLOAT_t entryPSetpoint = {&masterConfig.profile[0].pidProfile.ptermSRateWeight, 0, 100, 1, 10};
static OSD_FLOAT_t entryPSetpoint = {&masterConfig.profile[0].pidProfile.setpointRelaxRatio, 0, 100, 1, 10};
static OSD_FLOAT_t entryDSetpoint = {&masterConfig.profile[0].pidProfile.dtermSetpointWeight, 0, 255, 1, 10};
OSD_Entry menuRateExpo[] =

2
src/main/io/serial_cli.c
View file

@ -851,7 +851,7 @@ const clivalue_t valueTable[] = {
{ "vbat_pid_compensation", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.vbatPidCompensation, .config.lookup = { TABLE_OFF_ON } },
{ "pid_at_min_throttle", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.pidAtMinThrottle, .config.lookup = { TABLE_OFF_ON } },
{ "iterm_throttle_gain", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermThrottleGain, .config.minmax = {0, 200 } },
{ "pterm_srate_ratio", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.ptermSRateWeight, .config.minmax = {0, 100 } },
{ "setpoint_relax_ratio", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.setpointRelaxRatio, .config.minmax = {0, 100 } },
{ "dterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dtermSetpointWeight, .config.minmax = {0, 255 } },
{ "yaw_rate_accel_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawRateAccelLimit, .config.minmax = {0, 1000 } },
{ "rate_accel_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rateAccelLimit, .config.minmax = {0, 1000 } },

4
src/main/io/serial_msp.c
View file

@ -1274,7 +1274,7 @@ static bool processOutCommand(uint8_t cmdMSP)
serialize16(currentProfile->pidProfile.yaw_p_limit);
serialize8(currentProfile->pidProfile.deltaMethod);
serialize8(currentProfile->pidProfile.vbatPidCompensation);
serialize8(currentProfile->pidProfile.ptermSRateWeight);
serialize8(currentProfile->pidProfile.setpointRelaxRatio);
serialize8(currentProfile->pidProfile.dtermSetpointWeight);
serialize8(0); // reserved
serialize8(0); // reserved
@ -1877,7 +1877,7 @@ static bool processInCommand(void)
currentProfile->pidProfile.yaw_p_limit = read16();
currentProfile->pidProfile.deltaMethod = read8();
currentProfile->pidProfile.vbatPidCompensation = read8();
currentProfile->pidProfile.ptermSRateWeight = read8();
currentProfile->pidProfile.setpointRelaxRatio = read8();
currentProfile->pidProfile.dtermSetpointWeight = read8();
read8(); // reserved
read8(); // reserved

14
src/main/mw.c
View file

@ -122,7 +122,7 @@ extern uint8_t PIDweight[3];
static bool isRXDataNew;
static bool armingCalibrationWasInitialised;
float setpointRate[3], ptermSetpointRate[3];
float setpointRate[3];
float rcInput[3];
extern pidControllerFuncPtr pid_controller;
@ -200,20 +200,8 @@ void calculateSetpointRate(int axis, int16_t rc) {
if (currentControlRateProfile->rates[axis]) {
rcSuperfactor = 1.0f / (constrainf(1.0f - (ABS(rcCommandf) * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f));
if (currentProfile->pidProfile.pidController == PID_CONTROLLER_BETAFLIGHT) {
ptermSetpointRate[axis] = constrainf(angleRate * rcSuperfactor, -1998.0f, 1998.0f);
if (currentProfile->pidProfile.ptermSRateWeight < 100 && axis != YAW && !flightModeFlags) {
const float pWeight = currentProfile->pidProfile.ptermSRateWeight / 100.0f;
angleRate = angleRate + (pWeight * ptermSetpointRate[axis] - angleRate);
} else {
angleRate = ptermSetpointRate[axis];
}
} else {
angleRate *= rcSuperfactor;
}
} else {
if (currentProfile->pidProfile.pidController == PID_CONTROLLER_BETAFLIGHT) ptermSetpointRate[axis] = angleRate;
}
if (debugMode == DEBUG_ANGLERATE) {
debug[axis] = angleRate;