mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-19 14:25:20 +03:00
Luxfloat rework to int pids // Many pid cleanups // filter rework
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:
parent
d95220327b
commit
7b468c09f0
12 changed files with 145 additions and 309 deletions
|
@ -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_1:
|
||||
case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_2:
|
||||
if (IS_PID_CONTROLLER_FP_BASED(currentProfile->pidProfile.pidController)) {
|
||||
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:
|
||||
#ifdef MAG
|
||||
|
|
|
@ -96,3 +96,26 @@ float applyBiQuadFilter(float sample, biquad_t *state)
|
|||
|
||||
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;
|
||||
}
|
||||
|
||||
|
|
|
@ -15,6 +15,8 @@
|
|||
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#define DELTA_MAX_SAMPLES 12
|
||||
|
||||
typedef struct filterStatePt1_s {
|
||||
float state;
|
||||
float RC;
|
||||
|
@ -29,4 +31,6 @@ typedef struct biquad_s {
|
|||
|
||||
float applyBiQuadFilter(float sample, biquad_t *state);
|
||||
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);
|
||||
|
|
|
@ -134,7 +134,7 @@ static uint32_t activeFeaturesLatch = 0;
|
|||
static uint8_t currentControlRateProfileIndex = 0;
|
||||
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)
|
||||
{
|
||||
|
@ -145,16 +145,16 @@ static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
|
|||
|
||||
static void resetPidProfile(pidProfile_t *pidProfile)
|
||||
{
|
||||
pidProfile->pidController = 1;
|
||||
pidProfile->pidController = 2;
|
||||
|
||||
pidProfile->P8[ROLL] = 42;
|
||||
pidProfile->I8[ROLL] = 40;
|
||||
pidProfile->P8[ROLL] = 40;
|
||||
pidProfile->I8[ROLL] = 30;
|
||||
pidProfile->D8[ROLL] = 18;
|
||||
pidProfile->P8[PITCH] = 54;
|
||||
pidProfile->I8[PITCH] = 40;
|
||||
pidProfile->D8[PITCH] = 22;
|
||||
pidProfile->P8[YAW] = 90;
|
||||
pidProfile->I8[YAW] = 50;
|
||||
pidProfile->P8[PITCH] = 40;
|
||||
pidProfile->I8[PITCH] = 30;
|
||||
pidProfile->D8[PITCH] = 18;
|
||||
pidProfile->P8[YAW] = 60;
|
||||
pidProfile->I8[YAW] = 40;
|
||||
pidProfile->D8[YAW] = 0;
|
||||
pidProfile->P8[PIDALT] = 50;
|
||||
pidProfile->I8[PIDALT] = 0;
|
||||
|
@ -168,8 +168,8 @@ static void resetPidProfile(pidProfile_t *pidProfile)
|
|||
pidProfile->P8[PIDNAVR] = 25; // NAV_P * 10;
|
||||
pidProfile->I8[PIDNAVR] = 33; // NAV_I * 100;
|
||||
pidProfile->D8[PIDNAVR] = 83; // NAV_D * 1000;
|
||||
pidProfile->P8[PIDLEVEL] = 30;
|
||||
pidProfile->I8[PIDLEVEL] = 30;
|
||||
pidProfile->P8[PIDLEVEL] = 50;
|
||||
pidProfile->I8[PIDLEVEL] = 50;
|
||||
pidProfile->D8[PIDLEVEL] = 100;
|
||||
pidProfile->P8[PIDMAG] = 40;
|
||||
pidProfile->P8[PIDVEL] = 120;
|
||||
|
@ -177,21 +177,11 @@ static void resetPidProfile(pidProfile_t *pidProfile)
|
|||
pidProfile->D8[PIDVEL] = 1;
|
||||
|
||||
pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX;
|
||||
pidProfile->yaw_lpf_hz = 0.0f;
|
||||
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->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;
|
||||
|
||||
#ifdef GTUNE
|
||||
|
@ -313,7 +303,7 @@ void resetSerialConfig(serialConfig_t *serialConfig)
|
|||
|
||||
static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) {
|
||||
controlRateConfig->rcRate8 = 100;
|
||||
controlRateConfig->rcExpo8 = 70;
|
||||
controlRateConfig->rcExpo8 = 20;
|
||||
controlRateConfig->thrMid8 = 50;
|
||||
controlRateConfig->thrExpo8 = 0;
|
||||
controlRateConfig->dynThrPID = 0;
|
||||
|
@ -321,7 +311,7 @@ static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) {
|
|||
controlRateConfig->tpa_breakpoint = 1500;
|
||||
|
||||
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.gyro_lpf = 0; // 256HZ default
|
||||
masterConfig.gyro_sync_denom = 8;
|
||||
masterConfig.gyro_soft_lpf_hz = 60;
|
||||
masterConfig.gyro_soft_lpf_hz = 80.0f;
|
||||
|
||||
masterConfig.pid_process_denom = 1;
|
||||
|
||||
|
|
|
@ -58,10 +58,9 @@ int16_t axisPID[3];
|
|||
int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
|
||||
#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
|
||||
uint8_t dynP8[3], dynI8[3], dynD8[3], PIDweight[3];
|
||||
float tpaFactor;
|
||||
|
||||
static int32_t errorGyroI[3], errorGyroILimit[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) {
|
||||
float rcCommandReflection = (float)rcCommand[axis] / 500.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 };
|
||||
|
||||
static biquad_t deltaBiQuadState[3];
|
||||
static bool deltaStateIsSet;
|
||||
static filterStatePt1_t deltaFilterState[3];
|
||||
static filterStatePt1_t yawFilterState;
|
||||
|
||||
static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
|
||||
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 ITerm,PTerm,DTerm;
|
||||
static float lastErrorForDelta[3];
|
||||
static float previousDelta[3][DELTA_MAX_SAMPLES];
|
||||
static float previousAverageDelta[3];
|
||||
float delta, deltaSum;
|
||||
int axis, deltaCount;
|
||||
static float deltaState[3][DELTA_MAX_SAMPLES];
|
||||
float delta;
|
||||
int axis;
|
||||
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)) {
|
||||
// Figure out the raw stick positions
|
||||
const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc));
|
||||
|
@ -191,11 +189,11 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
|
|||
#endif
|
||||
if (FLIGHT_MODE(ANGLE_MODE)) {
|
||||
// ANGLE mode - control is angle based, so control loop is needed
|
||||
AngleRate = errorAngle * pidProfile->A_level;
|
||||
AngleRate = 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
|
||||
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
|
||||
RateError = AngleRate - gyroRate;
|
||||
|
||||
if (lowThrottlePidReduction) RateError /= 4;
|
||||
|
||||
// -----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
|
||||
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.
|
||||
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(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
|
||||
// would be scaled by different dt each time. Division by dT fixes that.
|
||||
delta *= (1.0f / dT);
|
||||
delta *= (1.0f / getdT());
|
||||
|
||||
// Filter delta
|
||||
if (pidProfile->dterm_lpf_hz) delta = filterApplyPt1(delta, &deltaFilterState[axis], pidProfile->dterm_lpf_hz, getdT());
|
||||
|
||||
if (deltaStateIsSet) {
|
||||
delta = applyBiQuadFilter(delta, &deltaBiQuadState[axis]);
|
||||
} 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;
|
||||
}
|
||||
if (pidProfile->dterm_average_count) delta = filterApplyAveragef(delta, pidProfile->dterm_average_count, deltaState[axis]);
|
||||
|
||||
DTerm = constrainf(delta * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f);
|
||||
DTerm = constrainf(delta * (float)pidProfile->D8[axis] * 0.001f * tpaFactor, -300.0f, 300.0f);
|
||||
|
||||
// -----calculate total PID output
|
||||
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
|
||||
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
|
||||
|
@ -285,17 +277,11 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
|
|||
{
|
||||
UNUSED(rxConfig);
|
||||
|
||||
int axis, deltaCount, prop = 0;
|
||||
int axis, prop = 0;
|
||||
int32_t rc, error, errorAngle, delta, gyroError;
|
||||
int32_t PTerm, ITerm, PTermACC, ITermACC, DTerm;
|
||||
static int16_t lastErrorForDelta[2];
|
||||
static int32_t previousDelta[2][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;
|
||||
}
|
||||
static int32_t deltaState[3][DELTA_MAX_SAMPLES];
|
||||
|
||||
if (FLIGHT_MODE(HORIZON_MODE)) {
|
||||
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;
|
||||
|
||||
if (lowThrottlePidReduction) rc /= 4;
|
||||
|
||||
gyroError = gyroADC[axis] / 4;
|
||||
|
||||
error = rc - gyroError;
|
||||
|
@ -368,23 +352,17 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
|
|||
// Scale delta to looptime
|
||||
delta = (delta * ((uint16_t) 0xFFFF)) / ((uint16_t)targetPidLooptime << 5);
|
||||
|
||||
if (deltaStateIsSet) {
|
||||
DTerm = lrintf(applyBiQuadFilter((float) delta, &deltaBiQuadState[axis])) * 3; // Keep same scaling as unfiltered delta
|
||||
} 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;
|
||||
}
|
||||
// Filer delta
|
||||
if (pidProfile->dterm_lpf_hz) delta = filterApplyPt1((float)delta, &deltaFilterState[axis], pidProfile->dterm_lpf_hz, getdT());
|
||||
|
||||
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;
|
||||
|
||||
if (lowThrottlePidReduction) axisPID[axis] /= 4;
|
||||
if (lowThrottlePidReduction) axisPID[axis] /= 3;
|
||||
|
||||
#ifdef GTUNE
|
||||
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;
|
||||
|
||||
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
|
||||
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
|
||||
|
@ -441,20 +419,14 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
|
|||
{
|
||||
UNUSED(rxConfig);
|
||||
|
||||
int axis, deltaCount;
|
||||
int32_t PTerm, ITerm, DTerm, delta, deltaSum;
|
||||
int axis;
|
||||
int32_t PTerm, ITerm, DTerm, delta;
|
||||
static int32_t lastErrorForDelta[3] = { 0, 0, 0 };
|
||||
static int32_t previousDelta[3][DELTA_MAX_SAMPLES];
|
||||
static int32_t previousAverageDelta[3];
|
||||
static int32_t deltaState[3][DELTA_MAX_SAMPLES];
|
||||
int32_t AngleRateTmp, RateError, gyroRate;
|
||||
|
||||
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)) {
|
||||
// Figure out the raw stick positions
|
||||
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;
|
||||
RateError = AngleRateTmp - gyroRate;
|
||||
|
||||
if (lowThrottlePidReduction) RateError /= 4;
|
||||
|
||||
// -----calculate P component
|
||||
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.
|
||||
delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetPidLooptime >> 4))) >> 6;
|
||||
|
||||
if (deltaStateIsSet) {
|
||||
delta = lrintf(applyBiQuadFilter((float) delta, &deltaBiQuadState[axis])) * 3; // Keep same scaling as unfiltered delta
|
||||
} else {
|
||||
// Filter delta
|
||||
if (pidProfile->dterm_lpf_hz) delta = filterApplyPt1((float)delta, &deltaFilterState[axis], pidProfile->dterm_lpf_hz, getdT());
|
||||
|
||||
// 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) << 1) + previousAverageDelta[axis]) >> 1; // Keep same original scaling + double pass averaging
|
||||
previousAverageDelta[axis] = delta;
|
||||
}
|
||||
if (pidProfile->dterm_average_count) delta = filterApplyAverage(delta, pidProfile->dterm_average_count, deltaState[axis]) * 2;
|
||||
|
||||
DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8;
|
||||
|
||||
// -----calculate total PID output
|
||||
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
|
||||
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
|
||||
|
|
|
@ -63,14 +63,10 @@ typedef struct pidProfile_s {
|
|||
uint8_t I8[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;
|
||||
|
||||
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
|
||||
uint16_t yaw_p_limit;
|
||||
uint8_t dterm_average_count; // Configurable delta count for dterm
|
||||
|
|
|
@ -700,30 +700,11 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
|
|||
bstWrite8(currentControlRateProfile->rcYawExpo8);
|
||||
break;
|
||||
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 < 3; i++) {
|
||||
bstWrite8(constrain(lrintf(currentProfile->pidProfile.P_f[i] * 10.0f), 0, 250));
|
||||
bstWrite8(constrain(lrintf(currentProfile->pidProfile.I_f[i] * 100.0f), 0, 250));
|
||||
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;
|
||||
case BST_PIDNAMES:
|
||||
bstWriteNames(pidnames);
|
||||
|
@ -1066,30 +1047,11 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
|
|||
pidSetController(currentProfile->pidProfile.pidController);
|
||||
break;
|
||||
case BST_SET_PID:
|
||||
if (IS_PID_CONTROLLER_FP_BASED(currentProfile->pidProfile.pidController)) {
|
||||
for (i = 0; i < 3; i++) {
|
||||
currentProfile->pidProfile.P_f[i] = (float)bstRead8() / 10.0f;
|
||||
currentProfile->pidProfile.I_f[i] = (float)bstRead8() / 100.0f;
|
||||
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;
|
||||
case BST_SET_MODE_RANGE:
|
||||
i = bstRead8();
|
||||
|
|
|
@ -476,7 +476,6 @@ void configureAdjustment(uint8_t index, uint8_t auxSwitchChannelIndex, const adj
|
|||
|
||||
void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int delta) {
|
||||
int newValue;
|
||||
float newFloatValue;
|
||||
|
||||
if (delta > 0) {
|
||||
beeperConfirmationBeeps(2);
|
||||
|
@ -523,114 +522,63 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm
|
|||
break;
|
||||
case ADJUSTMENT_PITCH_ROLL_P:
|
||||
case ADJUSTMENT_PITCH_P:
|
||||
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
|
||||
newFloatValue = constrainf(pidProfile->P_f[PIDPITCH] + (float)(delta / 100.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c
|
||||
pidProfile->P_f[PIDPITCH] = newFloatValue;
|
||||
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) {
|
||||
break;
|
||||
}
|
||||
// follow though for combined ADJUSTMENT_PITCH_ROLL_P
|
||||
case ADJUSTMENT_ROLL_P:
|
||||
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
|
||||
newFloatValue = constrainf(pidProfile->P_f[PIDROLL] + (float)(delta / 100.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c
|
||||
pidProfile->P_f[PIDROLL] = newFloatValue;
|
||||
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;
|
||||
case ADJUSTMENT_PITCH_ROLL_I:
|
||||
case ADJUSTMENT_PITCH_I:
|
||||
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
|
||||
newFloatValue = constrainf(pidProfile->I_f[PIDPITCH] + (float)(delta / 100.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c
|
||||
pidProfile->I_f[PIDPITCH] = newFloatValue;
|
||||
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) {
|
||||
break;
|
||||
}
|
||||
// follow though for combined ADJUSTMENT_PITCH_ROLL_I
|
||||
case ADJUSTMENT_ROLL_I:
|
||||
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
|
||||
newFloatValue = constrainf(pidProfile->I_f[PIDROLL] + (float)(delta / 100.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c
|
||||
pidProfile->I_f[PIDROLL] = newFloatValue;
|
||||
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;
|
||||
case ADJUSTMENT_PITCH_ROLL_D:
|
||||
case ADJUSTMENT_PITCH_D:
|
||||
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
|
||||
newFloatValue = constrainf(pidProfile->D_f[PIDPITCH] + (float)(delta / 1000.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c
|
||||
pidProfile->D_f[PIDPITCH] = newFloatValue;
|
||||
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) {
|
||||
break;
|
||||
}
|
||||
// follow though for combined ADJUSTMENT_PITCH_ROLL_D
|
||||
case ADJUSTMENT_ROLL_D:
|
||||
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
|
||||
newFloatValue = constrainf(pidProfile->D_f[PIDROLL] + (float)(delta / 1000.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c
|
||||
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);
|
||||
}
|
||||
break;
|
||||
case ADJUSTMENT_YAW_P:
|
||||
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
|
||||
newFloatValue = constrainf(pidProfile->P_f[PIDYAW] + (float)(delta / 100.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c
|
||||
pidProfile->P_f[PIDYAW] = newFloatValue;
|
||||
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;
|
||||
case ADJUSTMENT_YAW_I:
|
||||
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
|
||||
newFloatValue = constrainf(pidProfile->I_f[PIDYAW] + (float)(delta / 100.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c
|
||||
pidProfile->I_f[PIDYAW] = newFloatValue;
|
||||
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;
|
||||
case ADJUSTMENT_YAW_D:
|
||||
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
|
||||
newFloatValue = constrainf(pidProfile->D_f[PIDYAW] + (float)(delta / 1000.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c
|
||||
pidProfile->D_f[PIDYAW] = newFloatValue;
|
||||
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;
|
||||
default:
|
||||
break;
|
||||
|
|
|
@ -728,7 +728,8 @@ const clivalue_t valueTable[] = {
|
|||
{ "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 } },
|
||||
{ "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_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 } },
|
||||
{ "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 } },
|
||||
|
||||
{ "p_alt", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PIDALT], .config.minmax = { 0, 200 } },
|
||||
|
|
|
@ -879,30 +879,11 @@ static bool processOutCommand(uint8_t cmdMSP)
|
|||
break;
|
||||
case MSP_PID:
|
||||
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 < 3; i++) {
|
||||
serialize8(constrain(lrintf(currentProfile->pidProfile.P_f[i] * 40.0f), 0, 255));
|
||||
serialize8(constrain(lrintf(currentProfile->pidProfile.I_f[i] * 100.0f), 0, 255));
|
||||
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;
|
||||
case MSP_PIDNAMES:
|
||||
headSerialReply(sizeof(pidnames) - 1);
|
||||
|
@ -1335,30 +1316,11 @@ static bool processInCommand(void)
|
|||
if (oldPid != currentProfile->pidProfile.pidController) setGyroSamplingSpeed(0); // recalculate looptimes for new PID
|
||||
break;
|
||||
case MSP_SET_PID:
|
||||
if (IS_PID_CONTROLLER_FP_BASED(currentProfile->pidProfile.pidController)) {
|
||||
for (i = 0; i < 3; i++) {
|
||||
currentProfile->pidProfile.P_f[i] = (float)read8() / 40.0f;
|
||||
currentProfile->pidProfile.I_f[i] = (float)read8() / 100.0f;
|
||||
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;
|
||||
case MSP_SET_MODE_RANGE:
|
||||
i = read8();
|
||||
|
|
|
@ -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
|
||||
|
||||
float dT;
|
||||
|
||||
int16_t magHold;
|
||||
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 uint8_t dynP8[3], dynI8[3], dynD8[3], PIDweight[3];
|
||||
extern float tpaFactor;
|
||||
extern bool antiWindupProtection;
|
||||
|
||||
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);
|
||||
} else {
|
||||
prop2 = 100 - currentControlRateProfile->dynThrPID;
|
||||
tpaFactor = prop2 / 100.0f;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -16,8 +16,8 @@
|
|||
*/
|
||||
|
||||
#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_PATCH_LEVEL 4 // increment when a bug is fixed
|
||||
#define FC_VERSION_MINOR 6 // increment when a minor release is made (small new feature, change etc)
|
||||
#define FC_VERSION_PATCH_LEVEL 0 // increment when a bug is fixed
|
||||
|
||||
#define STR_HELPER(x) #x
|
||||
#define STR(x) STR_HELPER(x)
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue