From 75693fbd5b27675437ce1fc96df4acafb7a54e0a Mon Sep 17 00:00:00 2001 From: mikeller Date: Sat, 2 Jun 2018 23:39:03 +1200 Subject: [PATCH] Added defaults and MSP support to filter settings. --- src/main/flight/pid.c | 4 ++-- src/main/interface/msp.c | 40 ++++++++++++++++++++++++++++++++++------ src/main/sensors/gyro.c | 6 +++--- src/main/sensors/gyro.h | 21 ++++++++++----------- 4 files changed, 49 insertions(+), 22 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 437d40017..f7bb9b623 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -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, diff --git a/src/main/interface/msp.c b/src/main/interface/msp.c index 841d71883..be827b00d 100644 --- a/src/main/interface/msp.c +++ b/src/main/interface/msp.c @@ -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); diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 53988afe1..f24adfd63 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -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, diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index f97dcaa60..415412aac 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -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);