parent
6d51ab6e53
commit
e490564815
|
@ -40,7 +40,7 @@ static int8_t gyroFIRCoeff_1000[3][9] = { { 0, 0, 12, 23, 40, 51, 52, 40, 38 },
|
|||
|
||||
int8_t * filterGetFIRCoefficientsTable(uint8_t filter_level)
|
||||
{
|
||||
return gyroFIRCoeff_1000[filter_level];
|
||||
return gyroFIRCoeff_1000[filter_level-1];
|
||||
}
|
||||
|
||||
// 9 Tap FIR filter as described here:
|
||||
|
|
|
@ -133,7 +133,7 @@ static uint32_t activeFeaturesLatch = 0;
|
|||
static uint8_t currentControlRateProfileIndex = 0;
|
||||
controlRateConfig_t *currentControlRateProfile;
|
||||
|
||||
static const uint8_t EEPROM_CONF_VERSION = 115;
|
||||
static const uint8_t EEPROM_CONF_VERSION = 116;
|
||||
|
||||
static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
|
||||
{
|
||||
|
@ -175,7 +175,7 @@ static void resetPidProfile(pidProfile_t *pidProfile)
|
|||
pidProfile->I8[PIDVEL] = 45;
|
||||
pidProfile->D8[PIDVEL] = 1;
|
||||
|
||||
pidProfile->gyro_soft_lpf = 0; // LOW filtering by default
|
||||
pidProfile->gyro_soft_lpf = 1; // LOW filtering by default
|
||||
pidProfile->dterm_cut_hz = 40;
|
||||
pidProfile->yaw_pterm_cut_hz = 50;
|
||||
|
||||
|
@ -399,7 +399,7 @@ static void resetConf(void)
|
|||
masterConfig.current_profile_index = 0; // default profile
|
||||
masterConfig.dcm_kp = 2500; // 1.0 * 10000
|
||||
masterConfig.dcm_ki = 0; // 0.003 * 10000
|
||||
masterConfig.gyro_lpf = 1; // 1KHZ or 8KHZ
|
||||
masterConfig.gyro_lpf = 1; // 188HZ
|
||||
|
||||
resetAccelerometerTrims(&masterConfig.accZero);
|
||||
|
||||
|
@ -694,7 +694,9 @@ void activateConfig(void)
|
|||
¤tProfile->pidProfile
|
||||
);
|
||||
|
||||
if (currentProfile->pidProfile.gyro_soft_lpf) {
|
||||
useGyroConfig(&masterConfig.gyroConfig, filterGetFIRCoefficientsTable(currentProfile->pidProfile.gyro_soft_lpf));
|
||||
}
|
||||
|
||||
#ifdef TELEMETRY
|
||||
telemetryUseConfig(&masterConfig.telemetryConfig);
|
||||
|
|
|
@ -96,7 +96,8 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
|
|||
float ITerm,PTerm,DTerm;
|
||||
int32_t stickPosAil, stickPosEle, mostDeflectedPos;
|
||||
static float lastError[3];
|
||||
float delta;
|
||||
static float delta1[3], delta2[3];
|
||||
float delta, deltaSum;
|
||||
int axis;
|
||||
float horizonLevelStrength = 1;
|
||||
static float previousErrorGyroIf[3] = { 0.0f, 0.0f, 0.0f };
|
||||
|
@ -198,12 +199,21 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
|
|||
// would be scaled by different dt each time. Division by dT fixes that.
|
||||
delta *= (1.0f / dT);
|
||||
|
||||
// Dterm low pass
|
||||
if (pidProfile->dterm_cut_hz) {
|
||||
delta = filterApplyPt1(delta, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
|
||||
if (!pidProfile->gyro_soft_lpf) {
|
||||
// add moving average here to reduce noise
|
||||
deltaSum = (delta1[axis] + delta2[axis] + delta) / 3;
|
||||
delta2[axis] = delta1[axis];
|
||||
delta1[axis] = delta;
|
||||
} else {
|
||||
deltaSum = delta;
|
||||
}
|
||||
|
||||
DTerm = constrainf(delta * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f);
|
||||
// Dterm low pass
|
||||
if (pidProfile->dterm_cut_hz) {
|
||||
deltaSum = filterApplyPt1(delta, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
|
||||
}
|
||||
|
||||
DTerm = constrainf(deltaSum * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f);
|
||||
|
||||
// -----calculate total PID output
|
||||
axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000);
|
||||
|
@ -229,7 +239,8 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
|
|||
|
||||
int32_t errorAngle;
|
||||
int axis;
|
||||
int32_t delta;
|
||||
int32_t delta, deltaSum;
|
||||
static int32_t delta1[3], delta2[3];
|
||||
int32_t PTerm, ITerm, DTerm;
|
||||
static int32_t lastError[3] = { 0, 0, 0 };
|
||||
static int32_t previousErrorGyroI[3] = { 0, 0, 0 };
|
||||
|
@ -335,12 +346,21 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
|
|||
// would be scaled by different dt each time. Division by dT fixes that.
|
||||
delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetLooptime >> 4))) >> 6;
|
||||
|
||||
// Dterm delta low pass
|
||||
if (pidProfile->dterm_cut_hz) {
|
||||
delta = filterApplyPt1(delta, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
|
||||
if (!pidProfile->gyro_soft_lpf) {
|
||||
// add moving average here to reduce noise
|
||||
deltaSum = delta1[axis] + delta2[axis] + delta;
|
||||
delta2[axis] = delta1[axis];
|
||||
delta1[axis] = delta;
|
||||
} else {
|
||||
deltaSum = delta * 2;
|
||||
}
|
||||
|
||||
DTerm = (delta * 2 * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8; // Multiplied by 2 to approximately match old scaling
|
||||
// Dterm delta low pass
|
||||
if (pidProfile->dterm_cut_hz) {
|
||||
deltaSum = filterApplyPt1(deltaSum, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
|
||||
}
|
||||
|
||||
DTerm = (deltaSum * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8;
|
||||
|
||||
// -----calculate total PID output
|
||||
axisPID[axis] = PTerm + ITerm + DTerm;
|
||||
|
|
|
@ -348,12 +348,14 @@ static const char * const lookupTableSerialRX[] = {
|
|||
};
|
||||
|
||||
static const char * const lookupTableGyroFilter[] = {
|
||||
"LOW", "MEDIUM", "HIGH"
|
||||
"OFF", "LOW", "MEDIUM", "HIGH"
|
||||
};
|
||||
|
||||
static const char * const lookupTableGyroSampling[] = {
|
||||
"8KHZ",
|
||||
"1KHZ"
|
||||
static const char * const lookupTableGyroLpf[] = {
|
||||
"OFF",
|
||||
"188HZ",
|
||||
"98HZ",
|
||||
"42HZ"
|
||||
};
|
||||
|
||||
typedef struct lookupTableEntry_s {
|
||||
|
@ -377,7 +379,7 @@ typedef enum {
|
|||
TABLE_PID_CONTROLLER,
|
||||
TABLE_SERIAL_RX,
|
||||
TABLE_GYRO_FILTER,
|
||||
TABLE_GYRO_SAMPLING,
|
||||
TABLE_GYRO_LPF,
|
||||
} lookupTableIndex_e;
|
||||
|
||||
static const lookupTableEntry_t lookupTables[] = {
|
||||
|
@ -394,7 +396,7 @@ static const lookupTableEntry_t lookupTables[] = {
|
|||
{ lookupTablePidController, sizeof(lookupTablePidController) / sizeof(char *) },
|
||||
{ lookupTableSerialRX, sizeof(lookupTableSerialRX) / sizeof(char *) },
|
||||
{ lookupTableGyroFilter, sizeof(lookupTableGyroFilter) / sizeof(char *) },
|
||||
{ lookupTableGyroSampling, sizeof(lookupTableGyroSampling) / sizeof(char *) }
|
||||
{ lookupTableGyroLpf, sizeof(lookupTableGyroLpf) / sizeof(char *) }
|
||||
};
|
||||
|
||||
#define VALUE_TYPE_OFFSET 0
|
||||
|
@ -545,7 +547,7 @@ const clivalue_t valueTable[] = {
|
|||
|
||||
{ "max_angle_inclination", VAR_UINT16 | MASTER_VALUE, &masterConfig.max_angle_inclination, .config.minmax = { 100, 900 } },
|
||||
|
||||
{ "gyro_sampling", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_SAMPLING } },
|
||||
{ "gyro_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } },
|
||||
{ "moron_threshold", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyroMovementCalibrationThreshold, .config.minmax = { 0, 128 } },
|
||||
{ "imu_dcm_kp", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_kp, .config.minmax = { 0, 50000 } },
|
||||
{ "imu_dcm_ki", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_ki, .config.minmax = { 0, 50000 } },
|
||||
|
|
Loading…
Reference in New Issue