rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Added defaults and MSP support to filter settings.

This commit is contained in:
mikeller 2018-06-02 23:39:03 +12:00
parent c1e88ae7f4
commit 75693fbd5b
4 changed files with 49 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/main/flight/pid.c
View File

@ -115,10 +115,10 @@ void resetPidProfile(pidProfile_t *pidProfile)
.pidSumLimitYaw = PIDSUM_LIMIT_YAW,
.yaw_lowpass_hz = 0,
.dterm_lowpass_hz = 100, // filtering ON by default
.dterm_lowpass2_hz = 0, // second Dterm LPF OFF by default
.dterm_lowpass2_hz = 200, // second Dterm LPF OFF by default
.dterm_notch_hz = 260,
.dterm_notch_cutoff = 160,
.dterm_filter_type = FILTER_BIQUAD,
.dterm_filter_type = FILTER_PT1,
.itermWindupPointPercent = 50,
.vbatPidCompensation = 0,
.pidAtMinThrottle = PID_STABILISATION_ON,

40
src/main/interface/msp.c
View File

@ -1230,8 +1230,14 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
sbufWriteU16(dst, motorConfig()->digitalIdleOffsetValue);
sbufWriteU8(dst, gyroConfig()->gyro_use_32khz);
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 :
sbufWriteU8(dst, gyroConfig()->gyro_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_cutoff_2);
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:
sbufWriteU16(dst, 0);
sbufWriteU16(dst, 0);
@ -1671,13 +1683,21 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
if (sbufBytesRemaining(src)) {
gyroConfigMutable()->gyro_use_32khz = sbufReadU8(src);
}
validateAndFixGyroConfig();
if (sbufBytesRemaining(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:
gyroConfigMutable()->gyro_lowpass_hz = sbufReadU8(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) {
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
validateAndFixGyroConfig();
gyroInitFilters();
// reinitialize the PID filters with the new values
pidInitFilters(currentPidProfile);
break;
break;
case MSP_SET_PID_ADVANCED:
sbufReadU16(src);
sbufReadU16(src);

6
src/main/sensors/gyro.c
View File

@ -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_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
#define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_1
@ -183,9 +183,9 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.gyro_hardware_lpf = GYRO_HARDWARE_LPF_NORMAL,
.gyro_32khz_hardware_lpf = GYRO_32KHZ_HARDWARE_LPF_NORMAL,
.gyro_lowpass_type = FILTER_PT1,
.gyro_lowpass_hz = 90,
.gyro_lowpass_hz = 100,
.gyro_lowpass2_type = FILTER_PT1,
.gyro_lowpass2_hz = 0,
.gyro_lowpass2_hz = 300,
.gyro_high_fsr = false,
.gyro_use_32khz = false,
.gyro_to_use = GYRO_CONFIG_USE_GYRO_DEFAULT,

21
src/main/sensors/gyro.h
View File

@ -68,35 +68,34 @@ typedef enum {
} filterSlots;
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 gyro_sync_denom; // Gyro sample divider
uint8_t gyro_hardware_lpf; // gyro DLPF setting
uint8_t gyro_32khz_hardware_lpf; // gyro 32khz DLPF setting
bool gyro_high_fsr;
bool gyro_use_32khz;
uint8_t gyro_high_fsr;
uint8_t gyro_use_32khz;
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_lowpass2_hz;
uint16_t gyro_lowpass2_hz;
uint16_t gyro_soft_notch_hz_1;
uint16_t gyro_soft_notch_cutoff_1;
uint16_t gyro_soft_notch_hz_2;
uint16_t gyro_soft_notch_cutoff_2;
gyroOverflowCheck_e checkOverflow;
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;
uint16_t gyroCalibrationDuration; // Gyro calibration duration in 1/100 second
} gyroConfig_t;
PG_DECLARE(gyroConfig_t, gyroConfig);