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

Luxfloat rework to int pids // Many pid cleanups // filter rework

Browse source 
Eeprom version // Dterm improvement

Further PID Improvements

Version Change

Coupling configured // reworked filtering // more test features

remove iterm scaler luxfloat

Further rework filters etc

Restore original luxfloat but scaled

Restore original luxfloat but scaled
This commit is contained in:
borisbstyle 2016-03-21 16:53:27 +01:00
parent d95220327b
commit 7b468c09f0
12 changed files with 145 additions and 309 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -401,11 +401,7 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0: case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0:
case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_1: case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_1:
case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_2: case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_2:
if (IS_PID_CONTROLLER_FP_BASED(currentProfile->pidProfile.pidController)) { return currentProfile->pidProfile.D8[condition - FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0] != 0;
return currentProfile->pidProfile.D_f[condition - FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0] != 0;
} else {
return currentProfile->pidProfile.D8[condition - FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0] != 0;
}
case FLIGHT_LOG_FIELD_CONDITION_MAG: case FLIGHT_LOG_FIELD_CONDITION_MAG:
#ifdef MAG #ifdef MAG

23
src/main/common/filter.c
View file

@ -96,3 +96,26 @@ float applyBiQuadFilter(float sample, biquad_t *state)
return result; return result;
} }
int32_t filterApplyAverage(int32_t input, uint8_t averageCount, int32_t averageState[DELTA_MAX_SAMPLES]) {
int count;
int32_t averageSum;
for (count = averageCount-1; count > 0; count--) averageState[count] = averageState[count-1];
averageState[0] = input;
for (count = 0; count < averageCount; count++) averageSum += averageState[count];
return averageSum / averageCount;
}
float filterApplyAveragef(float input, uint8_t averageCount, float averageState[DELTA_MAX_SAMPLES]) {
int count;
float averageSum;
for (count = averageCount-1; count > 0; count--) averageState[count] = averageState[count-1];
averageState[0] = input;
for (count = 0; count < averageCount; count++) averageSum += averageState[count];
return averageSum / averageCount;
}

4
src/main/common/filter.h
View file

@ -15,6 +15,8 @@
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>. * along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/ */
#define DELTA_MAX_SAMPLES 12
typedef struct filterStatePt1_s { typedef struct filterStatePt1_s {
float state; float state;
float RC; float RC;
@ -29,4 +31,6 @@ typedef struct biquad_s {
float applyBiQuadFilter(float sample, biquad_t *state); float applyBiQuadFilter(float sample, biquad_t *state);
float filterApplyPt1(float input, filterStatePt1_t *filter, float f_cut, float dt); float filterApplyPt1(float input, filterStatePt1_t *filter, float f_cut, float dt);
int32_t filterApplyAverage(int32_t input, uint8_t averageCount, int32_t averageState[DELTA_MAX_SAMPLES]);
float filterApplyAveragef(float input, uint8_t averageCount, float averageState[DELTA_MAX_SAMPLES]);
void BiQuadNewLpf(float filterCutFreq, biquad_t *newState, uint32_t refreshRate); void BiQuadNewLpf(float filterCutFreq, biquad_t *newState, uint32_t refreshRate);

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

@ -134,7 +134,7 @@ static uint32_t activeFeaturesLatch = 0;
static uint8_t currentControlRateProfileIndex = 0; static uint8_t currentControlRateProfileIndex = 0;
controlRateConfig_t *currentControlRateProfile; controlRateConfig_t *currentControlRateProfile;
static const uint8_t EEPROM_CONF_VERSION = 129; static const uint8_t EEPROM_CONF_VERSION = 130;
static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
{ {
@ -145,16 +145,16 @@ static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
static void resetPidProfile(pidProfile_t *pidProfile) static void resetPidProfile(pidProfile_t *pidProfile)
{ {
pidProfile->pidController = 1; pidProfile->pidController = 2;
pidProfile->P8[ROLL] = 42; pidProfile->P8[ROLL] = 40;
pidProfile->I8[ROLL] = 40; pidProfile->I8[ROLL] = 30;
pidProfile->D8[ROLL] = 18; pidProfile->D8[ROLL] = 18;
pidProfile->P8[PITCH] = 54; pidProfile->P8[PITCH] = 40;
pidProfile->I8[PITCH] = 40; pidProfile->I8[PITCH] = 30;
pidProfile->D8[PITCH] = 22; pidProfile->D8[PITCH] = 18;
pidProfile->P8[YAW] = 90; pidProfile->P8[YAW] = 60;
pidProfile->I8[YAW] = 50; pidProfile->I8[YAW] = 40;
pidProfile->D8[YAW] = 0; pidProfile->D8[YAW] = 0;
pidProfile->P8[PIDALT] = 50; pidProfile->P8[PIDALT] = 50;
pidProfile->I8[PIDALT] = 0; pidProfile->I8[PIDALT] = 0;
@ -168,8 +168,8 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->P8[PIDNAVR] = 25; // NAV_P * 10; pidProfile->P8[PIDNAVR] = 25; // NAV_P * 10;
pidProfile->I8[PIDNAVR] = 33; // NAV_I * 100; pidProfile->I8[PIDNAVR] = 33; // NAV_I * 100;
pidProfile->D8[PIDNAVR] = 83; // NAV_D * 1000; pidProfile->D8[PIDNAVR] = 83; // NAV_D * 1000;
pidProfile->P8[PIDLEVEL] = 30; pidProfile->P8[PIDLEVEL] = 50;
pidProfile->I8[PIDLEVEL] = 30; pidProfile->I8[PIDLEVEL] = 50;
pidProfile->D8[PIDLEVEL] = 100; pidProfile->D8[PIDLEVEL] = 100;
pidProfile->P8[PIDMAG] = 40; pidProfile->P8[PIDMAG] = 40;
pidProfile->P8[PIDVEL] = 120; pidProfile->P8[PIDVEL] = 120;
@ -177,21 +177,11 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->D8[PIDVEL] = 1; pidProfile->D8[PIDVEL] = 1;
pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX; pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX;
pidProfile->yaw_lpf_hz = 0.0f;
pidProfile->dterm_average_count = 4; pidProfile->dterm_average_count = 4;
pidProfile->dterm_lpf_hz = 0; // filtering ON by default pidProfile->dterm_lpf_hz = 70.0f; // filtering ON by default
pidProfile->deltaMethod = DELTA_FROM_MEASUREMENT; pidProfile->deltaMethod = DELTA_FROM_MEASUREMENT;
pidProfile->P_f[ROLL] = 1.1f;
pidProfile->I_f[ROLL] = 0.4f;
pidProfile->D_f[ROLL] = 0.01f;
pidProfile->P_f[PITCH] = 1.4f;
pidProfile->I_f[PITCH] = 0.4f;
pidProfile->D_f[PITCH] = 0.01f;
pidProfile->P_f[YAW] = 2.5f;
pidProfile->I_f[YAW] = 0.4f;
pidProfile->D_f[YAW] = 0.00f;
pidProfile->A_level = 4.0f;
pidProfile->H_level = 4.0f;
pidProfile->H_sensitivity = 75; pidProfile->H_sensitivity = 75;
#ifdef GTUNE #ifdef GTUNE
@ -313,7 +303,7 @@ void resetSerialConfig(serialConfig_t *serialConfig)
static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) { static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) {
controlRateConfig->rcRate8 = 100; controlRateConfig->rcRate8 = 100;
controlRateConfig->rcExpo8 = 70; controlRateConfig->rcExpo8 = 20;
controlRateConfig->thrMid8 = 50; controlRateConfig->thrMid8 = 50;
controlRateConfig->thrExpo8 = 0; controlRateConfig->thrExpo8 = 0;
controlRateConfig->dynThrPID = 0; controlRateConfig->dynThrPID = 0;
@ -321,7 +311,7 @@ static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) {
controlRateConfig->tpa_breakpoint = 1500; controlRateConfig->tpa_breakpoint = 1500;
for (uint8_t axis = 0; axis < FLIGHT_DYNAMICS_INDEX_COUNT; axis++) { for (uint8_t axis = 0; axis < FLIGHT_DYNAMICS_INDEX_COUNT; axis++) {
controlRateConfig->rates[axis] = 0; controlRateConfig->rates[axis] = 50;
} }
} }
@ -411,7 +401,7 @@ static void resetConf(void)
masterConfig.dcm_ki = 0; // 0.003 * 10000 masterConfig.dcm_ki = 0; // 0.003 * 10000
masterConfig.gyro_lpf = 0; // 256HZ default masterConfig.gyro_lpf = 0; // 256HZ default
masterConfig.gyro_sync_denom = 8; masterConfig.gyro_sync_denom = 8;
masterConfig.gyro_soft_lpf_hz = 60; masterConfig.gyro_soft_lpf_hz = 80.0f;
masterConfig.pid_process_denom = 1; masterConfig.pid_process_denom = 1;

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

@ -58,10 +58,9 @@ int16_t axisPID[3];
int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
#endif #endif
#define DELTA_MAX_SAMPLES 12
// PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction // 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 dynP8[3], dynI8[3], dynD8[3], PIDweight[3]; uint8_t dynP8[3], dynI8[3], dynD8[3], PIDweight[3];
float tpaFactor;
static int32_t errorGyroI[3], errorGyroILimit[3]; static int32_t errorGyroI[3], errorGyroILimit[3];
static float errorGyroIf[3], errorGyroIfLimit[3]; static float errorGyroIf[3], errorGyroIfLimit[3];
@ -107,6 +106,13 @@ void pidResetErrorGyroState(uint8_t resetOption)
} }
} }
float getdT (void) {
static float dT;
if (!dT) dT = (float)targetPidLooptime * 0.000001f;
return dT;
}
void scaleItermToRcInput(int axis, pidProfile_t *pidProfile) { void scaleItermToRcInput(int axis, pidProfile_t *pidProfile) {
float rcCommandReflection = (float)rcCommand[axis] / 500.0f; float rcCommandReflection = (float)rcCommand[axis] / 500.0f;
static float iTermScaler[3] = {1.0f, 1.0f, 1.0f}; static float iTermScaler[3] = {1.0f, 1.0f, 1.0f};
@ -134,8 +140,8 @@ void scaleItermToRcInput(int axis, pidProfile_t *pidProfile) {
const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH }; const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
static biquad_t deltaBiQuadState[3]; static filterStatePt1_t deltaFilterState[3];
static bool deltaStateIsSet; static filterStatePt1_t yawFilterState;
static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
@ -143,19 +149,11 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
float RateError, AngleRate, gyroRate; float RateError, AngleRate, gyroRate;
float ITerm,PTerm,DTerm; float ITerm,PTerm,DTerm;
static float lastErrorForDelta[3]; static float lastErrorForDelta[3];
static float previousDelta[3][DELTA_MAX_SAMPLES]; static float deltaState[3][DELTA_MAX_SAMPLES];
static float previousAverageDelta[3]; float delta;
float delta, deltaSum; int axis;
int axis, deltaCount;
float horizonLevelStrength = 1; float horizonLevelStrength = 1;
float dT = (float)targetPidLooptime * 0.000001f;
if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) {
for (axis = 0; axis < 3; axis++) BiQuadNewLpf(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], targetPidLooptime);
deltaStateIsSet = true;
}
if (FLIGHT_MODE(HORIZON_MODE)) { if (FLIGHT_MODE(HORIZON_MODE)) {
// Figure out the raw stick positions // Figure out the raw stick positions
const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc)); const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc));
@ -191,11 +189,11 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
#endif #endif
if (FLIGHT_MODE(ANGLE_MODE)) { if (FLIGHT_MODE(ANGLE_MODE)) {
// ANGLE mode - control is angle based, so control loop is needed // ANGLE mode - control is angle based, so control loop is needed
AngleRate = errorAngle * pidProfile->A_level; AngleRate = errorAngle * pidProfile->P8[PIDLEVEL] / 10.0f;
} else { } else {
// HORIZON mode - direct sticks control is applied to rate PID // HORIZON mode - direct sticks control is applied to rate PID
// mix up angle error to desired AngleRate to add a little auto-level feel // mix up angle error to desired AngleRate to add a little auto-level feel
AngleRate += errorAngle * pidProfile->H_level * horizonLevelStrength; AngleRate += errorAngle * pidProfile->I8[PIDLEVEL] / 10.0f * horizonLevelStrength;
} }
} }
} }
@ -208,10 +206,8 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
// multiplication of rcCommand corresponds to changing the sticks scaling here // multiplication of rcCommand corresponds to changing the sticks scaling here
RateError = AngleRate - gyroRate; RateError = AngleRate - gyroRate;
if (lowThrottlePidReduction) RateError /= 4;
// -----calculate P component // -----calculate P component
PTerm = RateError * pidProfile->P_f[axis] * PIDweight[axis] / 100; PTerm = RateError * (pidProfile->P8[axis] / 40.0f) * tpaFactor;
// Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply // Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply
if((motorCount >= 4 && pidProfile->yaw_p_limit) && axis == YAW) { if((motorCount >= 4 && pidProfile->yaw_p_limit) && axis == YAW) {
@ -219,7 +215,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
} }
// -----calculate I component. // -----calculate I component.
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis] * 10, -250.0f, 250.0f); errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * getdT() * pidProfile->I8[axis] / 10.0f, -250.0f, 250.0f);
if (IS_RC_MODE_ACTIVE(BOXAIRMODE) || IS_RC_MODE_ACTIVE(BOXACROPLUS)) { if (IS_RC_MODE_ACTIVE(BOXAIRMODE) || IS_RC_MODE_ACTIVE(BOXACROPLUS)) {
if (IS_RC_MODE_ACTIVE(BOXACROPLUS)) scaleItermToRcInput(axis, pidProfile); if (IS_RC_MODE_ACTIVE(BOXACROPLUS)) scaleItermToRcInput(axis, pidProfile);
@ -245,26 +241,22 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
// Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
// would be scaled by different dt each time. Division by dT fixes that. // would be scaled by different dt each time. Division by dT fixes that.
delta *= (1.0f / dT); delta *= (1.0f / getdT());
if (deltaStateIsSet) { // Filter delta
delta = applyBiQuadFilter(delta, &deltaBiQuadState[axis]); if (pidProfile->dterm_lpf_hz) delta = filterApplyPt1(delta, &deltaFilterState[axis], pidProfile->dterm_lpf_hz, getdT());
} else {
// Apply moving average
deltaSum = 0;
for (deltaCount = pidProfile->dterm_average_count-1; deltaCount > 0; deltaCount--) previousDelta[axis][deltaCount] = previousDelta[axis][deltaCount-1];
previousDelta[axis][0] = delta;
for (deltaCount = 0; deltaCount < pidProfile->dterm_average_count; deltaCount++) deltaSum += previousDelta[axis][deltaCount];
delta = ((deltaSum / pidProfile->dterm_average_count) + previousAverageDelta[axis]) / 2; // Keep same original scaling + double pass averaging
previousAverageDelta[axis] = delta;
}
DTerm = constrainf(delta * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f); // Apply moving average
if (pidProfile->dterm_average_count) delta = filterApplyAveragef(delta, pidProfile->dterm_average_count, deltaState[axis]);
DTerm = constrainf(delta * (float)pidProfile->D8[axis] * 0.001f * tpaFactor, -300.0f, 300.0f);
// -----calculate total PID output // -----calculate total PID output
axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000); axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000);
if (lowThrottlePidReduction) axisPID[axis] /= 4; if (pidProfile->yaw_lpf_hz && axis == YAW) axisPID[axis] = filterApplyPt1(axisPID[axis], &yawFilterState, pidProfile->yaw_lpf_hz, getdT());
if (lowThrottlePidReduction) axisPID[axis] /= 3;
#ifdef GTUNE #ifdef GTUNE
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
@ -285,17 +277,11 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
{ {
UNUSED(rxConfig); UNUSED(rxConfig);
int axis, deltaCount, prop = 0; int axis, prop = 0;
int32_t rc, error, errorAngle, delta, gyroError; int32_t rc, error, errorAngle, delta, gyroError;
int32_t PTerm, ITerm, PTermACC, ITermACC, DTerm; int32_t PTerm, ITerm, PTermACC, ITermACC, DTerm;
static int16_t lastErrorForDelta[2]; static int16_t lastErrorForDelta[2];
static int32_t previousDelta[2][DELTA_MAX_SAMPLES]; static int32_t deltaState[3][DELTA_MAX_SAMPLES];
static int32_t previousAverageDelta[2];
if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) {
for (axis = 0; axis < 2; axis++) BiQuadNewLpf(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], targetPidLooptime);
deltaStateIsSet = true;
}
if (FLIGHT_MODE(HORIZON_MODE)) { if (FLIGHT_MODE(HORIZON_MODE)) {
prop = MIN(MAX(ABS(rcCommand[PITCH]), ABS(rcCommand[ROLL])), 512); prop = MIN(MAX(ABS(rcCommand[PITCH]), ABS(rcCommand[ROLL])), 512);
@ -306,8 +292,6 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
rc = rcCommand[axis] << 1; rc = rcCommand[axis] << 1;
if (lowThrottlePidReduction) rc /= 4;
gyroError = gyroADC[axis] / 4; gyroError = gyroADC[axis] / 4;
error = rc - gyroError; error = rc - gyroError;
@ -368,23 +352,17 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
// Scale delta to looptime // Scale delta to looptime
delta = (delta * ((uint16_t) 0xFFFF)) / ((uint16_t)targetPidLooptime << 5); delta = (delta * ((uint16_t) 0xFFFF)) / ((uint16_t)targetPidLooptime << 5);
if (deltaStateIsSet) { // Filer delta
DTerm = lrintf(applyBiQuadFilter((float) delta, &deltaBiQuadState[axis])) * 3; // Keep same scaling as unfiltered delta if (pidProfile->dterm_lpf_hz) delta = filterApplyPt1((float)delta, &deltaFilterState[axis], pidProfile->dterm_lpf_hz, getdT());
} else {
// Apply moving average
DTerm = 0;
for (deltaCount = pidProfile->dterm_average_count-1; deltaCount > 0; deltaCount--) previousDelta[axis][deltaCount] = previousDelta[axis][deltaCount-1];
previousDelta[axis][0] = delta;
for (deltaCount = 0; deltaCount < pidProfile->dterm_average_count; deltaCount++) DTerm += previousDelta[axis][deltaCount];
DTerm = (((DTerm / pidProfile->dterm_average_count) << 1) + previousAverageDelta[axis]) >> 1; // Keep same original scaling + double pass averaging
previousAverageDelta[axis] = DTerm;
}
DTerm = ((int32_t)DTerm * dynD8[axis]) >> 5; // 32 bits is needed for calculation // Apply moving average and multiply to get original scaling
if (pidProfile->dterm_average_count) delta = filterApplyAverage(delta, pidProfile->dterm_average_count, deltaState[axis]) * 2;
DTerm = (delta * dynD8[axis]) >> 5; // 32 bits is needed for calculation
axisPID[axis] = PTerm + ITerm + DTerm; axisPID[axis] = PTerm + ITerm + DTerm;
if (lowThrottlePidReduction) axisPID[axis] /= 4; if (lowThrottlePidReduction) axisPID[axis] /= 3;
#ifdef GTUNE #ifdef GTUNE
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
@ -421,7 +399,7 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
axisPID[FD_YAW] = PTerm + ITerm; axisPID[FD_YAW] = PTerm + ITerm;
if (lowThrottlePidReduction) axisPID[FD_YAW] /= 4; if (pidProfile->yaw_lpf_hz) axisPID[FD_YAW] = filterApplyPt1(axisPID[FD_YAW], &yawFilterState, pidProfile->yaw_lpf_hz, getdT());
#ifdef GTUNE #ifdef GTUNE
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
@ -441,20 +419,14 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
{ {
UNUSED(rxConfig); UNUSED(rxConfig);
int axis, deltaCount; int axis;
int32_t PTerm, ITerm, DTerm, delta, deltaSum; int32_t PTerm, ITerm, DTerm, delta;
static int32_t lastErrorForDelta[3] = { 0, 0, 0 }; static int32_t lastErrorForDelta[3] = { 0, 0, 0 };
static int32_t previousDelta[3][DELTA_MAX_SAMPLES]; static int32_t deltaState[3][DELTA_MAX_SAMPLES];
static int32_t previousAverageDelta[3];
int32_t AngleRateTmp, RateError, gyroRate; int32_t AngleRateTmp, RateError, gyroRate;
int8_t horizonLevelStrength = 100; int8_t horizonLevelStrength = 100;
if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) {
for (axis = 0; axis < 3; axis++) BiQuadNewLpf(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], targetPidLooptime);
deltaStateIsSet = true;
}
if (FLIGHT_MODE(HORIZON_MODE)) { if (FLIGHT_MODE(HORIZON_MODE)) {
// Figure out the raw stick positions // Figure out the raw stick positions
const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc)); const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc));
@ -504,8 +476,6 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
gyroRate = gyroADC[axis] / 4; gyroRate = gyroADC[axis] / 4;
RateError = AngleRateTmp - gyroRate; RateError = AngleRateTmp - gyroRate;
if (lowThrottlePidReduction) RateError /= 4;
// -----calculate P component // -----calculate P component
PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7; PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7;
@ -549,24 +519,20 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
// would be scaled by different dt each time. Division by dT fixes that. // would be scaled by different dt each time. Division by dT fixes that.
delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetPidLooptime >> 4))) >> 6; delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetPidLooptime >> 4))) >> 6;
if (deltaStateIsSet) { // Filter delta
delta = lrintf(applyBiQuadFilter((float) delta, &deltaBiQuadState[axis])) * 3; // Keep same scaling as unfiltered delta if (pidProfile->dterm_lpf_hz) delta = filterApplyPt1((float)delta, &deltaFilterState[axis], pidProfile->dterm_lpf_hz, getdT());
} else {
// Apply moving average // Apply moving average
deltaSum = 0; if (pidProfile->dterm_average_count) delta = filterApplyAverage(delta, pidProfile->dterm_average_count, deltaState[axis]) * 2;
for (deltaCount = pidProfile->dterm_average_count -1; deltaCount > 0; deltaCount--) previousDelta[axis][deltaCount] = previousDelta[axis][deltaCount-1];
previousDelta[axis][0] = delta;
for (deltaCount = 0; deltaCount < pidProfile->dterm_average_count; deltaCount++) deltaSum += previousDelta[axis][deltaCount];
delta = (((deltaSum / pidProfile->dterm_average_count) << 1) + previousAverageDelta[axis]) >> 1; // Keep same original scaling + double pass averaging
previousAverageDelta[axis] = delta;
}
DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8; DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8;
// -----calculate total PID output // -----calculate total PID output
axisPID[axis] = PTerm + ITerm + DTerm; axisPID[axis] = PTerm + ITerm + DTerm;
if (lowThrottlePidReduction) axisPID[axis] /= 4; if (pidProfile->yaw_lpf_hz && axis == YAW) axisPID[axis] = filterApplyPt1(axisPID[axis], &yawFilterState, pidProfile->yaw_lpf_hz, getdT());
if (lowThrottlePidReduction) axisPID[axis] /= 3;
#ifdef GTUNE #ifdef GTUNE
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {

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

@ -63,14 +63,10 @@ typedef struct pidProfile_s {
uint8_t I8[PID_ITEM_COUNT]; uint8_t I8[PID_ITEM_COUNT];
uint8_t D8[PID_ITEM_COUNT]; uint8_t D8[PID_ITEM_COUNT];
float P_f[3]; // float p i and d factors for lux float pid controller
float I_f[3];
float D_f[3];
float A_level;
float H_level;
uint8_t H_sensitivity; uint8_t H_sensitivity;
float dterm_lpf_hz; // Delta Filter in hz float dterm_lpf_hz; // Delta Filter in hz
float yaw_lpf_hz; // Additional yaw filter when yaw axis too noisy
uint8_t deltaMethod; // Alternative delta Calculation uint8_t deltaMethod; // Alternative delta Calculation
uint16_t yaw_p_limit; uint16_t yaw_p_limit;
uint8_t dterm_average_count; // Configurable delta count for dterm uint8_t dterm_average_count; // Configurable delta count for dterm

56
src/main/io/i2c_bst.c
View file

@ -700,29 +700,10 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
bstWrite8(currentControlRateProfile->rcYawExpo8); bstWrite8(currentControlRateProfile->rcYawExpo8);
break; break;
case BST_PID: case BST_PID:
if (IS_PID_CONTROLLER_FP_BASED(currentProfile->pidProfile.pidController)) { // convert float stuff into uint8_t to keep backwards compatability with all 8-bit shit with new pid for (i = 0; i < PID_ITEM_COUNT; i++) {
for (i = 0; i < 3; i++) { bstWrite8(currentProfile->pidProfile.P8[i]);
bstWrite8(constrain(lrintf(currentProfile->pidProfile.P_f[i] * 10.0f), 0, 250)); bstWrite8(currentProfile->pidProfile.I8[i]);
bstWrite8(constrain(lrintf(currentProfile->pidProfile.I_f[i] * 100.0f), 0, 250)); bstWrite8(currentProfile->pidProfile.D8[i]);
bstWrite8(constrain(lrintf(currentProfile->pidProfile.D_f[i] * 1000.0f), 0, 100));
}
for (i = 3; i < PID_ITEM_COUNT; i++) {
if (i == PIDLEVEL) {
bstWrite8(constrain(lrintf(currentProfile->pidProfile.A_level * 10.0f), 0, 250));
bstWrite8(constrain(lrintf(currentProfile->pidProfile.H_level * 10.0f), 0, 250));
bstWrite8(constrain(lrintf(currentProfile->pidProfile.H_sensitivity), 0, 250));
} else {
bstWrite8(currentProfile->pidProfile.P8[i]);
bstWrite8(currentProfile->pidProfile.I8[i]);
bstWrite8(currentProfile->pidProfile.D8[i]);
}
}
} else {
for (i = 0; i < PID_ITEM_COUNT; i++) {
bstWrite8(currentProfile->pidProfile.P8[i]);
bstWrite8(currentProfile->pidProfile.I8[i]);
bstWrite8(currentProfile->pidProfile.D8[i]);
}
} }
break; break;
case BST_PIDNAMES: case BST_PIDNAMES:
@ -1066,30 +1047,11 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
pidSetController(currentProfile->pidProfile.pidController); pidSetController(currentProfile->pidProfile.pidController);
break; break;
case BST_SET_PID: case BST_SET_PID:
if (IS_PID_CONTROLLER_FP_BASED(currentProfile->pidProfile.pidController)) { for (i = 0; i < PID_ITEM_COUNT; i++) {
for (i = 0; i < 3; i++) { currentProfile->pidProfile.P8[i] = bstRead8();
currentProfile->pidProfile.P_f[i] = (float)bstRead8() / 10.0f; currentProfile->pidProfile.I8[i] = bstRead8();
currentProfile->pidProfile.I_f[i] = (float)bstRead8() / 100.0f; currentProfile->pidProfile.D8[i] = bstRead8();
currentProfile->pidProfile.D_f[i] = (float)bstRead8() / 1000.0f; }
}
for (i = 3; i < PID_ITEM_COUNT; i++) {
if (i == PIDLEVEL) {
currentProfile->pidProfile.A_level = (float)bstRead8() / 10.0f;
currentProfile->pidProfile.H_level = (float)bstRead8() / 10.0f;
currentProfile->pidProfile.H_sensitivity = bstRead8();
} else {
currentProfile->pidProfile.P8[i] = bstRead8();
currentProfile->pidProfile.I8[i] = bstRead8();
currentProfile->pidProfile.D8[i] = bstRead8();
}
}
} else {
for (i = 0; i < PID_ITEM_COUNT; i++) {
currentProfile->pidProfile.P8[i] = bstRead8();
currentProfile->pidProfile.I8[i] = bstRead8();
currentProfile->pidProfile.D8[i] = bstRead8();
}
}
break; break;
case BST_SET_MODE_RANGE: case BST_SET_MODE_RANGE:
i = bstRead8(); i = bstRead8();

110
src/main/io/rc_controls.c
View file

@ -476,7 +476,6 @@ void configureAdjustment(uint8_t index, uint8_t auxSwitchChannelIndex, const adj
void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int delta) { void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int delta) {
int newValue; int newValue;
float newFloatValue;
if (delta > 0) { if (delta > 0) {
beeperConfirmationBeeps(2); beeperConfirmationBeeps(2);
@ -523,114 +522,63 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm
break; break;
case ADJUSTMENT_PITCH_ROLL_P: case ADJUSTMENT_PITCH_ROLL_P:
case ADJUSTMENT_PITCH_P: case ADJUSTMENT_PITCH_P:
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { newValue = constrain((int)pidProfile->P8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
newFloatValue = constrainf(pidProfile->P_f[PIDPITCH] + (float)(delta / 100.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c pidProfile->P8[PIDPITCH] = newValue;
pidProfile->P_f[PIDPITCH] = newFloatValue; blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_P, newValue);
blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_PITCH_P, newFloatValue);
} else {
newValue = constrain((int)pidProfile->P8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
pidProfile->P8[PIDPITCH] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_P, newValue);
}
if (adjustmentFunction == ADJUSTMENT_PITCH_P) { if (adjustmentFunction == ADJUSTMENT_PITCH_P) {
break; break;
} }
// follow though for combined ADJUSTMENT_PITCH_ROLL_P // follow though for combined ADJUSTMENT_PITCH_ROLL_P
case ADJUSTMENT_ROLL_P: case ADJUSTMENT_ROLL_P:
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { newValue = constrain((int)pidProfile->P8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
newFloatValue = constrainf(pidProfile->P_f[PIDROLL] + (float)(delta / 100.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c pidProfile->P8[PIDROLL] = newValue;
pidProfile->P_f[PIDROLL] = newFloatValue; blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_P, newValue);
blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_ROLL_P, newFloatValue);
} else {
newValue = constrain((int)pidProfile->P8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
pidProfile->P8[PIDROLL] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_P, newValue);
}
break; break;
case ADJUSTMENT_PITCH_ROLL_I: case ADJUSTMENT_PITCH_ROLL_I:
case ADJUSTMENT_PITCH_I: case ADJUSTMENT_PITCH_I:
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { newValue = constrain((int)pidProfile->I8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
newFloatValue = constrainf(pidProfile->I_f[PIDPITCH] + (float)(delta / 100.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c pidProfile->I8[PIDPITCH] = newValue;
pidProfile->I_f[PIDPITCH] = newFloatValue; blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_I, newValue);
blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_PITCH_I, newFloatValue);
} else {
newValue = constrain((int)pidProfile->I8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
pidProfile->I8[PIDPITCH] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_I, newValue);
}
if (adjustmentFunction == ADJUSTMENT_PITCH_I) { if (adjustmentFunction == ADJUSTMENT_PITCH_I) {
break; break;
} }
// follow though for combined ADJUSTMENT_PITCH_ROLL_I // follow though for combined ADJUSTMENT_PITCH_ROLL_I
case ADJUSTMENT_ROLL_I: case ADJUSTMENT_ROLL_I:
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { newValue = constrain((int)pidProfile->I8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
newFloatValue = constrainf(pidProfile->I_f[PIDROLL] + (float)(delta / 100.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c pidProfile->I8[PIDROLL] = newValue;
pidProfile->I_f[PIDROLL] = newFloatValue; blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_I, newValue);
blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_ROLL_I, newFloatValue);
} else {
newValue = constrain((int)pidProfile->I8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
pidProfile->I8[PIDROLL] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_I, newValue);
}
break; break;
case ADJUSTMENT_PITCH_ROLL_D: case ADJUSTMENT_PITCH_ROLL_D:
case ADJUSTMENT_PITCH_D: case ADJUSTMENT_PITCH_D:
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { newValue = constrain((int)pidProfile->D8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
newFloatValue = constrainf(pidProfile->D_f[PIDPITCH] + (float)(delta / 1000.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c pidProfile->D8[PIDPITCH] = newValue;
pidProfile->D_f[PIDPITCH] = newFloatValue; blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_D, newValue);
blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_PITCH_D, newFloatValue);
} else {
newValue = constrain((int)pidProfile->D8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
pidProfile->D8[PIDPITCH] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_D, newValue);
}
if (adjustmentFunction == ADJUSTMENT_PITCH_D) { if (adjustmentFunction == ADJUSTMENT_PITCH_D) {
break; break;
} }
// follow though for combined ADJUSTMENT_PITCH_ROLL_D // follow though for combined ADJUSTMENT_PITCH_ROLL_D
case ADJUSTMENT_ROLL_D: case ADJUSTMENT_ROLL_D:
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { newValue = constrain((int)pidProfile->D8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
newFloatValue = constrainf(pidProfile->D_f[PIDROLL] + (float)(delta / 1000.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c pidProfile->D8[PIDROLL] = newValue;
pidProfile->D_f[PIDROLL] = newFloatValue;
blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_ROLL_D, newFloatValue);
} else {
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; break;
case ADJUSTMENT_YAW_P: case ADJUSTMENT_YAW_P:
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { newValue = constrain((int)pidProfile->P8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
newFloatValue = constrainf(pidProfile->P_f[PIDYAW] + (float)(delta / 100.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c pidProfile->P8[PIDYAW] = newValue;
pidProfile->P_f[PIDYAW] = newFloatValue; blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_P, newValue);
blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_YAW_P, newFloatValue);
} else {
newValue = constrain((int)pidProfile->P8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
pidProfile->P8[PIDYAW] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_P, newValue);
}
break; break;
case ADJUSTMENT_YAW_I: case ADJUSTMENT_YAW_I:
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { newValue = constrain((int)pidProfile->I8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
newFloatValue = constrainf(pidProfile->I_f[PIDYAW] + (float)(delta / 100.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c pidProfile->I8[PIDYAW] = newValue;
pidProfile->I_f[PIDYAW] = newFloatValue; blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_I, newValue);
blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_YAW_I, newFloatValue);
} else {
newValue = constrain((int)pidProfile->I8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
pidProfile->I8[PIDYAW] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_I, newValue);
}
break; break;
case ADJUSTMENT_YAW_D: case ADJUSTMENT_YAW_D:
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { newValue = constrain((int)pidProfile->D8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
newFloatValue = constrainf(pidProfile->D_f[PIDYAW] + (float)(delta / 1000.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c pidProfile->D8[PIDYAW] = newValue;
pidProfile->D_f[PIDYAW] = newFloatValue; blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_D, newValue);
blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_YAW_D, newFloatValue);
} else {
newValue = constrain((int)pidProfile->D8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
pidProfile->D8[PIDYAW] = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_D, newValue);
}
break; break;
default: default:
break; break;

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

@ -728,7 +728,8 @@ const clivalue_t valueTable[] = {
{ "mag_declination", VAR_INT16 | MASTER_VALUE, &masterConfig.mag_declination, .config.minmax = { -18000, 18000 } }, { "mag_declination", VAR_INT16 | MASTER_VALUE, &masterConfig.mag_declination, .config.minmax = { -18000, 18000 } },
{ "pid_delta_method", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.deltaMethod, .config.lookup = { TABLE_DELTA_METHOD } }, { "pid_delta_method", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.deltaMethod, .config.lookup = { TABLE_DELTA_METHOD } },
{ "dterm_lpf_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } }, { "dterm_lpf_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } },
{ "dterm_average_count", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_average_count, .config.minmax = {2, 12 } }, { "dterm_average_count", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_average_count, .config.minmax = {0, 12 } },
{ "yaw_lpf_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } },
{ "pid_process_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.pid_process_denom, .config.minmax = { 1, 8 } }, { "pid_process_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.pid_process_denom, .config.minmax = { 1, 8 } },
{ "pid_controller", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.pidController, .config.lookup = { TABLE_PID_CONTROLLER } }, { "pid_controller", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.pidController, .config.lookup = { TABLE_PID_CONTROLLER } },
@ -743,18 +744,6 @@ const clivalue_t valueTable[] = {
{ "i_yaw", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[YAW], .config.minmax = { 0, 200 } }, { "i_yaw", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[YAW], .config.minmax = { 0, 200 } },
{ "d_yaw", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[YAW], .config.minmax = { 0, 200 } }, { "d_yaw", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[YAW], .config.minmax = { 0, 200 } },
{ "p_pitchf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P_f[PITCH], .config.minmax = { 0, 100 } },
{ "i_pitchf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I_f[PITCH], .config.minmax = { 0, 100 } },
{ "d_pitchf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D_f[PITCH], .config.minmax = { 0, 100 } },
{ "p_rollf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P_f[ROLL], .config.minmax = { 0, 100 } },
{ "i_rollf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I_f[ROLL], .config.minmax = { 0, 100 } },
{ "d_rollf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D_f[ROLL], .config.minmax = { 0, 100 } },
{ "p_yawf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P_f[YAW], .config.minmax = { 0, 100 } },
{ "i_yawf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I_f[YAW], .config.minmax = { 0, 100 } },
{ "d_yawf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D_f[YAW], .config.minmax = { 0, 100 } },
{ "level_horizon", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.H_level, .config.minmax = { 0, 10 } },
{ "level_angle", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.A_level, .config.minmax = { 0, 10 } },
{ "sensitivity_horizon", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.H_sensitivity, .config.minmax = { 0, 250 } }, { "sensitivity_horizon", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.H_sensitivity, .config.minmax = { 0, 250 } },
{ "p_alt", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PIDALT], .config.minmax = { 0, 200 } }, { "p_alt", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PIDALT], .config.minmax = { 0, 200 } },

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

@ -879,29 +879,10 @@ static bool processOutCommand(uint8_t cmdMSP)
break; break;
case MSP_PID: case MSP_PID:
headSerialReply(3 * PID_ITEM_COUNT); headSerialReply(3 * PID_ITEM_COUNT);
if (IS_PID_CONTROLLER_FP_BASED(currentProfile->pidProfile.pidController)) { // convert float stuff into uint8_t to keep backwards compatability with all 8-bit shit with new pid for (i = 0; i < PID_ITEM_COUNT; i++) {
for (i = 0; i < 3; i++) { serialize8(currentProfile->pidProfile.P8[i]);
serialize8(constrain(lrintf(currentProfile->pidProfile.P_f[i] * 40.0f), 0, 255)); serialize8(currentProfile->pidProfile.I8[i]);
serialize8(constrain(lrintf(currentProfile->pidProfile.I_f[i] * 100.0f), 0, 255)); serialize8(currentProfile->pidProfile.D8[i]);
serialize8(constrain(lrintf(currentProfile->pidProfile.D_f[i] * 4000.0f), 0, 255));
}
for (i = 3; i < PID_ITEM_COUNT; i++) {
if (i == PIDLEVEL) {
serialize8(constrain(lrintf(currentProfile->pidProfile.A_level * 10.0f), 0, 255));
serialize8(constrain(lrintf(currentProfile->pidProfile.H_level * 10.0f), 0, 255));
serialize8(constrain(lrintf(currentProfile->pidProfile.H_sensitivity), 0, 255));
} else {
serialize8(currentProfile->pidProfile.P8[i]);
serialize8(currentProfile->pidProfile.I8[i]);
serialize8(currentProfile->pidProfile.D8[i]);
}
}
} else {
for (i = 0; i < PID_ITEM_COUNT; i++) {
serialize8(currentProfile->pidProfile.P8[i]);
serialize8(currentProfile->pidProfile.I8[i]);
serialize8(currentProfile->pidProfile.D8[i]);
}
} }
break; break;
case MSP_PIDNAMES: case MSP_PIDNAMES:
@ -1335,29 +1316,10 @@ static bool processInCommand(void)
if (oldPid != currentProfile->pidProfile.pidController) setGyroSamplingSpeed(0); // recalculate looptimes for new PID if (oldPid != currentProfile->pidProfile.pidController) setGyroSamplingSpeed(0); // recalculate looptimes for new PID
break; break;
case MSP_SET_PID: case MSP_SET_PID:
if (IS_PID_CONTROLLER_FP_BASED(currentProfile->pidProfile.pidController)) { for (i = 0; i < PID_ITEM_COUNT; i++) {
for (i = 0; i < 3; i++) { currentProfile->pidProfile.P8[i] = read8();
currentProfile->pidProfile.P_f[i] = (float)read8() / 40.0f; currentProfile->pidProfile.I8[i] = read8();
currentProfile->pidProfile.I_f[i] = (float)read8() / 100.0f; currentProfile->pidProfile.D8[i] = read8();
currentProfile->pidProfile.D_f[i] = (float)read8() / 4000.0f;
}
for (i = 3; i < PID_ITEM_COUNT; i++) {
if (i == PIDLEVEL) {
currentProfile->pidProfile.A_level = (float)read8() / 10.0f;
currentProfile->pidProfile.H_level = (float)read8() / 10.0f;
currentProfile->pidProfile.H_sensitivity = read8();
} else {
currentProfile->pidProfile.P8[i] = read8();
currentProfile->pidProfile.I8[i] = read8();
currentProfile->pidProfile.D8[i] = read8();
}
}
} else {
for (i = 0; i < PID_ITEM_COUNT; i++) {
currentProfile->pidProfile.P8[i] = read8();
currentProfile->pidProfile.I8[i] = read8();
currentProfile->pidProfile.D8[i] = read8();
}
} }
break; break;
case MSP_SET_MODE_RANGE: case MSP_SET_MODE_RANGE:

4
src/main/mw.c
View file

@ -103,8 +103,6 @@ enum {
uint16_t cycleTime = 0; // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop uint16_t cycleTime = 0; // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop
float dT;
int16_t magHold; int16_t magHold;
int16_t headFreeModeHold; int16_t headFreeModeHold;
@ -115,6 +113,7 @@ static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the m
extern uint32_t currentTime; extern uint32_t currentTime;
extern uint8_t dynP8[3], dynI8[3], dynD8[3], PIDweight[3]; extern uint8_t dynP8[3], dynI8[3], dynD8[3], PIDweight[3];
extern float tpaFactor;
extern bool antiWindupProtection; extern bool antiWindupProtection;
uint16_t filteredCycleTime; uint16_t filteredCycleTime;
@ -234,6 +233,7 @@ void annexCode(void)
prop2 = 100 - (uint16_t)currentControlRateProfile->dynThrPID * (rcData[THROTTLE] - currentControlRateProfile->tpa_breakpoint) / (2000 - currentControlRateProfile->tpa_breakpoint); prop2 = 100 - (uint16_t)currentControlRateProfile->dynThrPID * (rcData[THROTTLE] - currentControlRateProfile->tpa_breakpoint) / (2000 - currentControlRateProfile->tpa_breakpoint);
} else { } else {
prop2 = 100 - currentControlRateProfile->dynThrPID; prop2 = 100 - currentControlRateProfile->dynThrPID;
tpaFactor = prop2 / 100.0f;
} }
} }

4
src/main/version.h
View file

@ -16,8 +16,8 @@
*/ */
#define FC_VERSION_MAJOR 2 // increment when a major release is made (big new feature, etc) #define FC_VERSION_MAJOR 2 // increment when a major release is made (big new feature, etc)
#define FC_VERSION_MINOR 5 // increment when a minor release is made (small new feature, change etc) #define FC_VERSION_MINOR 6 // increment when a minor release is made (small new feature, change etc)
#define FC_VERSION_PATCH_LEVEL 4 // increment when a bug is fixed #define FC_VERSION_PATCH_LEVEL 0 // increment when a bug is fixed
#define STR_HELPER(x) #x #define STR_HELPER(x) #x
#define STR(x) STR_HELPER(x) #define STR(x) STR_HELPER(x)