Merge pull request #6024 from mikeller/add_filter_msp_and_defaults
Added defaults and MSP support to filter settings.
This commit is contained in:
commit
b21b626e12
|
@ -115,7 +115,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
|
||||||
.pidSumLimitYaw = PIDSUM_LIMIT_YAW,
|
.pidSumLimitYaw = PIDSUM_LIMIT_YAW,
|
||||||
.yaw_lowpass_hz = 0,
|
.yaw_lowpass_hz = 0,
|
||||||
.dterm_lowpass_hz = 100, // filtering ON by default
|
.dterm_lowpass_hz = 100, // filtering ON by default
|
||||||
.dterm_lowpass2_hz = 0, // second Dterm LPF OFF by default
|
.dterm_lowpass2_hz = 0, // second Dterm LPF OFF by default
|
||||||
.dterm_notch_hz = 260,
|
.dterm_notch_hz = 260,
|
||||||
.dterm_notch_cutoff = 160,
|
.dterm_notch_cutoff = 160,
|
||||||
.dterm_filter_type = FILTER_BIQUAD,
|
.dterm_filter_type = FILTER_BIQUAD,
|
||||||
|
|
|
@ -1230,8 +1230,14 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
|
||||||
sbufWriteU16(dst, motorConfig()->digitalIdleOffsetValue);
|
sbufWriteU16(dst, motorConfig()->digitalIdleOffsetValue);
|
||||||
sbufWriteU8(dst, gyroConfig()->gyro_use_32khz);
|
sbufWriteU8(dst, gyroConfig()->gyro_use_32khz);
|
||||||
sbufWriteU8(dst, motorConfig()->dev.motorPwmInversion);
|
sbufWriteU8(dst, motorConfig()->dev.motorPwmInversion);
|
||||||
break;
|
sbufWriteU8(dst, gyroConfig()->gyro_to_use);
|
||||||
|
sbufWriteU8(dst, gyroConfig()->gyro_high_fsr);
|
||||||
|
sbufWriteU8(dst, gyroConfig()->gyroMovementCalibrationThreshold);
|
||||||
|
sbufWriteU16(dst, gyroConfig()->gyroCalibrationDuration);
|
||||||
|
sbufWriteU16(dst, gyroConfig()->gyro_offset_yaw);
|
||||||
|
sbufWriteU8(dst, gyroConfig()->checkOverflow);
|
||||||
|
|
||||||
|
break;
|
||||||
case MSP_FILTER_CONFIG :
|
case MSP_FILTER_CONFIG :
|
||||||
sbufWriteU8(dst, gyroConfig()->gyro_lowpass_hz);
|
sbufWriteU8(dst, gyroConfig()->gyro_lowpass_hz);
|
||||||
sbufWriteU16(dst, currentPidProfile->dterm_lowpass_hz);
|
sbufWriteU16(dst, currentPidProfile->dterm_lowpass_hz);
|
||||||
|
@ -1243,8 +1249,14 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
|
||||||
sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_hz_2);
|
sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_hz_2);
|
||||||
sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_cutoff_2);
|
sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_cutoff_2);
|
||||||
sbufWriteU8(dst, currentPidProfile->dterm_filter_type);
|
sbufWriteU8(dst, currentPidProfile->dterm_filter_type);
|
||||||
break;
|
sbufWriteU8(dst, gyroConfig()->gyro_hardware_lpf);
|
||||||
|
sbufWriteU8(dst, gyroConfig()->gyro_32khz_hardware_lpf);
|
||||||
|
sbufWriteU16(dst, gyroConfig()->gyro_lowpass2_hz);
|
||||||
|
sbufWriteU8(dst, gyroConfig()->gyro_lowpass_type);
|
||||||
|
sbufWriteU8(dst, gyroConfig()->gyro_lowpass2_type);
|
||||||
|
sbufWriteU16(dst, currentPidProfile->dterm_lowpass2_hz);
|
||||||
|
|
||||||
|
break;
|
||||||
case MSP_PID_ADVANCED:
|
case MSP_PID_ADVANCED:
|
||||||
sbufWriteU16(dst, 0);
|
sbufWriteU16(dst, 0);
|
||||||
sbufWriteU16(dst, 0);
|
sbufWriteU16(dst, 0);
|
||||||
|
@ -1671,13 +1683,21 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
|
||||||
if (sbufBytesRemaining(src)) {
|
if (sbufBytesRemaining(src)) {
|
||||||
gyroConfigMutable()->gyro_use_32khz = sbufReadU8(src);
|
gyroConfigMutable()->gyro_use_32khz = sbufReadU8(src);
|
||||||
}
|
}
|
||||||
validateAndFixGyroConfig();
|
|
||||||
|
|
||||||
if (sbufBytesRemaining(src)) {
|
if (sbufBytesRemaining(src)) {
|
||||||
motorConfigMutable()->dev.motorPwmInversion = sbufReadU8(src);
|
motorConfigMutable()->dev.motorPwmInversion = sbufReadU8(src);
|
||||||
}
|
}
|
||||||
break;
|
if (sbufBytesRemaining(src) >= 8) {
|
||||||
|
gyroConfigMutable()->gyro_to_use = sbufReadU8(src);
|
||||||
|
gyroConfigMutable()->gyro_high_fsr = sbufReadU8(src);
|
||||||
|
gyroConfigMutable()->gyroMovementCalibrationThreshold = sbufReadU8(src);
|
||||||
|
gyroConfigMutable()->gyroCalibrationDuration = sbufReadU16(src);
|
||||||
|
gyroConfigMutable()->gyro_offset_yaw = sbufReadU16(src);
|
||||||
|
gyroConfigMutable()->checkOverflow = sbufReadU8(src);
|
||||||
|
}
|
||||||
|
|
||||||
|
validateAndFixGyroConfig();
|
||||||
|
|
||||||
|
break;
|
||||||
case MSP_SET_FILTER_CONFIG:
|
case MSP_SET_FILTER_CONFIG:
|
||||||
gyroConfigMutable()->gyro_lowpass_hz = sbufReadU8(src);
|
gyroConfigMutable()->gyro_lowpass_hz = sbufReadU8(src);
|
||||||
currentPidProfile->dterm_lowpass_hz = sbufReadU16(src);
|
currentPidProfile->dterm_lowpass_hz = sbufReadU16(src);
|
||||||
|
@ -1695,13 +1715,21 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
|
||||||
if (sbufBytesRemaining(src) >= 1) {
|
if (sbufBytesRemaining(src) >= 1) {
|
||||||
currentPidProfile->dterm_filter_type = sbufReadU8(src);
|
currentPidProfile->dterm_filter_type = sbufReadU8(src);
|
||||||
}
|
}
|
||||||
|
if (sbufBytesRemaining(src) >= 8) {
|
||||||
|
gyroConfigMutable()->gyro_hardware_lpf = sbufReadU8(src);
|
||||||
|
gyroConfigMutable()->gyro_32khz_hardware_lpf = sbufReadU8(src);
|
||||||
|
gyroConfigMutable()->gyro_lowpass2_hz = sbufReadU16(src);
|
||||||
|
gyroConfigMutable()->gyro_lowpass_type = sbufReadU8(src);
|
||||||
|
gyroConfigMutable()->gyro_lowpass2_type = sbufReadU8(src);
|
||||||
|
currentPidProfile->dterm_lowpass2_hz = sbufReadU16(src);
|
||||||
|
}
|
||||||
// reinitialize the gyro filters with the new values
|
// reinitialize the gyro filters with the new values
|
||||||
validateAndFixGyroConfig();
|
validateAndFixGyroConfig();
|
||||||
gyroInitFilters();
|
gyroInitFilters();
|
||||||
// reinitialize the PID filters with the new values
|
// reinitialize the PID filters with the new values
|
||||||
pidInitFilters(currentPidProfile);
|
pidInitFilters(currentPidProfile);
|
||||||
break;
|
|
||||||
|
|
||||||
|
break;
|
||||||
case MSP_SET_PID_ADVANCED:
|
case MSP_SET_PID_ADVANCED:
|
||||||
sbufReadU16(src);
|
sbufReadU16(src);
|
||||||
sbufReadU16(src);
|
sbufReadU16(src);
|
||||||
|
|
|
@ -169,7 +169,7 @@ static void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int typ
|
||||||
#define GYRO_OVERFLOW_TRIGGER_THRESHOLD 31980 // 97.5% full scale (1950dps for 2000dps gyro)
|
#define GYRO_OVERFLOW_TRIGGER_THRESHOLD 31980 // 97.5% full scale (1950dps for 2000dps gyro)
|
||||||
#define GYRO_OVERFLOW_RESET_THRESHOLD 30340 // 92.5% full scale (1850dps for 2000dps gyro)
|
#define GYRO_OVERFLOW_RESET_THRESHOLD 30340 // 92.5% full scale (1850dps for 2000dps gyro)
|
||||||
|
|
||||||
PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 3);
|
PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 4);
|
||||||
|
|
||||||
#ifndef GYRO_CONFIG_USE_GYRO_DEFAULT
|
#ifndef GYRO_CONFIG_USE_GYRO_DEFAULT
|
||||||
#define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_1
|
#define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_1
|
||||||
|
|
|
@ -68,35 +68,34 @@ typedef enum {
|
||||||
} filterSlots;
|
} filterSlots;
|
||||||
|
|
||||||
typedef struct gyroConfig_s {
|
typedef struct gyroConfig_s {
|
||||||
sensor_align_e gyro_align; // gyro alignment
|
uint8_t gyro_align; // gyro alignment
|
||||||
uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
|
uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
|
||||||
uint8_t gyro_sync_denom; // Gyro sample divider
|
uint8_t gyro_sync_denom; // Gyro sample divider
|
||||||
uint8_t gyro_hardware_lpf; // gyro DLPF setting
|
uint8_t gyro_hardware_lpf; // gyro DLPF setting
|
||||||
uint8_t gyro_32khz_hardware_lpf; // gyro 32khz DLPF setting
|
uint8_t gyro_32khz_hardware_lpf; // gyro 32khz DLPF setting
|
||||||
|
|
||||||
bool gyro_high_fsr;
|
uint8_t gyro_high_fsr;
|
||||||
bool gyro_use_32khz;
|
uint8_t gyro_use_32khz;
|
||||||
uint8_t gyro_to_use;
|
uint8_t gyro_to_use;
|
||||||
|
|
||||||
// Lowpass primary/secondary
|
|
||||||
uint8_t gyro_lowpass_type;
|
|
||||||
uint8_t gyro_lowpass2_type;
|
|
||||||
|
|
||||||
uint16_t gyro_lowpass_hz;
|
uint16_t gyro_lowpass_hz;
|
||||||
uint16_t gyro_lowpass2_hz;
|
uint16_t gyro_lowpass2_hz;
|
||||||
|
|
||||||
uint16_t gyro_soft_notch_hz_1;
|
uint16_t gyro_soft_notch_hz_1;
|
||||||
uint16_t gyro_soft_notch_cutoff_1;
|
uint16_t gyro_soft_notch_cutoff_1;
|
||||||
uint16_t gyro_soft_notch_hz_2;
|
uint16_t gyro_soft_notch_hz_2;
|
||||||
uint16_t gyro_soft_notch_cutoff_2;
|
uint16_t gyro_soft_notch_cutoff_2;
|
||||||
gyroOverflowCheck_e checkOverflow;
|
|
||||||
int16_t gyro_offset_yaw;
|
int16_t gyro_offset_yaw;
|
||||||
|
uint8_t checkOverflow;
|
||||||
|
|
||||||
bool yaw_spin_recovery;
|
// Lowpass primary/secondary
|
||||||
|
uint8_t gyro_lowpass_type;
|
||||||
|
uint8_t gyro_lowpass2_type;
|
||||||
|
|
||||||
|
uint8_t yaw_spin_recovery;
|
||||||
int16_t yaw_spin_threshold;
|
int16_t yaw_spin_threshold;
|
||||||
|
|
||||||
uint16_t gyroCalibrationDuration; // Gyro calibration duration in 1/100 second
|
uint16_t gyroCalibrationDuration; // Gyro calibration duration in 1/100 second
|
||||||
|
|
||||||
} gyroConfig_t;
|
} gyroConfig_t;
|
||||||
|
|
||||||
PG_DECLARE(gyroConfig_t, gyroConfig);
|
PG_DECLARE(gyroConfig_t, gyroConfig);
|
||||||
|
|
Loading…
Reference in New Issue