diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 50e4097c5..57a979f0c 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1291,8 +1291,10 @@ static bool blackboxWriteSysinfo() BLACKBOX_PRINT_HEADER_LINE("gyro_lpf:%d", masterConfig.gyro_lpf); BLACKBOX_PRINT_HEADER_LINE("gyro_soft_type:%d", masterConfig.gyro_soft_type); BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz:%d", (int)(masterConfig.gyro_soft_lpf_hz * 100.0f)); - BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz:%d", (int)(masterConfig.gyro_soft_notch_hz * 100.0f)); - BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff:%d", (int)(masterConfig.gyro_soft_notch_cutoff * 100.0f)); + BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz_1:%d", (int)(masterConfig.gyro_soft_notch_hz_1 * 100.0f)); + BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff_1:%d", (int)(masterConfig.gyro_soft_notch_cutoff_1 * 100.0f)); + BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz_2:%d", (int)(masterConfig.gyro_soft_notch_hz_2 * 100.0f)); + BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff_2:%d", (int)(masterConfig.gyro_soft_notch_cutoff_2 * 100.0f)); BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz:%d", (int)(masterConfig.acc_lpf_hz * 100.0f)); BLACKBOX_PRINT_HEADER_LINE("acc_hardware:%d", masterConfig.acc_hardware); BLACKBOX_PRINT_HEADER_LINE("baro_hardware:%d", masterConfig.baro_hardware); diff --git a/src/main/config/config.c b/src/main/config/config.c index f5f9f57a7..ef6ac66bd 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -207,10 +207,10 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->P8[ROLL] = 45; pidProfile->I8[ROLL] = 40; - pidProfile->D8[ROLL] = 20; + pidProfile->D8[ROLL] = 16; pidProfile->P8[PITCH] = 60; pidProfile->I8[PITCH] = 65; - pidProfile->D8[PITCH] = 22; + pidProfile->D8[PITCH] = 19; pidProfile->P8[YAW] = 70; pidProfile->I8[YAW] = 45; pidProfile->D8[YAW] = 20; @@ -487,9 +487,11 @@ void createDefaultConfig(master_t *config) config->pid_process_denom = 2; #endif config->gyro_soft_type = FILTER_PT1; - config->gyro_soft_lpf_hz = 90; - config->gyro_soft_notch_hz = 0; - config->gyro_soft_notch_cutoff = 130; + config->gyro_soft_lpf_hz = 80; + config->gyro_soft_notch_hz_1 = 400; + config->gyro_soft_notch_cutoff_1 = 300; + config->gyro_soft_notch_hz_2 = 0; + config->gyro_soft_notch_cutoff_2 = 100; config->debug_mode = DEBUG_NONE; @@ -762,7 +764,20 @@ void activateConfig(void) ¤tProfile->pidProfile ); - gyroUseConfig(&masterConfig.gyroConfig, masterConfig.gyro_soft_lpf_hz, masterConfig.gyro_soft_notch_hz, masterConfig.gyro_soft_notch_cutoff, masterConfig.gyro_soft_type); + // Prevent invalid notch cutoff + if (masterConfig.gyro_soft_notch_cutoff_1 >= masterConfig.gyro_soft_notch_hz_1) + masterConfig.gyro_soft_notch_hz_1 = 0; + + if (masterConfig.gyro_soft_notch_cutoff_2 >= masterConfig.gyro_soft_notch_hz_2) + masterConfig.gyro_soft_notch_hz_2 = 0; + + gyroUseConfig(&masterConfig.gyroConfig, + masterConfig.gyro_soft_lpf_hz, + masterConfig.gyro_soft_notch_hz_1, + masterConfig.gyro_soft_notch_cutoff_1, + masterConfig.gyro_soft_notch_hz_2, + masterConfig.gyro_soft_notch_cutoff_2, + masterConfig.gyro_soft_type); #ifdef TELEMETRY telemetryUseConfig(&masterConfig.telemetryConfig); diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index da8a6dd63..df0b6805d 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -59,8 +59,10 @@ typedef struct master_t { uint8_t gyro_sync_denom; // Gyro sample divider uint8_t gyro_soft_type; // Gyro Filter Type uint8_t gyro_soft_lpf_hz; // Biquad gyro lpf hz - uint16_t gyro_soft_notch_hz; // Biquad gyro notch hz - uint16_t gyro_soft_notch_cutoff; // Biquad gyro notch low cutoff + uint16_t gyro_soft_notch_hz_1; // Biquad gyro notch hz + uint16_t gyro_soft_notch_cutoff_1; // Biquad gyro notch low cutoff + uint16_t gyro_soft_notch_hz_2; // Biquad gyro notch hz + uint16_t gyro_soft_notch_cutoff_2; // Biquad gyro notch low cutoff uint16_t dcm_kp; // DCM filter proportional gain ( x 10000) uint16_t dcm_ki; // DCM filter integral gain ( x 10000) diff --git a/src/main/io/msp_protocol.h b/src/main/io/msp_protocol.h index 25f821d05..8d14b4d69 100644 --- a/src/main/io/msp_protocol.h +++ b/src/main/io/msp_protocol.h @@ -59,7 +59,7 @@ #define MSP_PROTOCOL_VERSION 0 #define API_VERSION_MAJOR 1 // increment when major changes are made -#define API_VERSION_MINOR 20 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR +#define API_VERSION_MINOR 21 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR #define API_VERSION_LENGTH 2 diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 526e3a9a2..489c66507 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -773,8 +773,10 @@ const clivalue_t valueTable[] = { { "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_sync_denom, .config.minmax = { 1, 8 } }, { "gyro_lowpass_level", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_soft_type, .config.lookup = { TABLE_LOWPASS_TYPE } }, { "gyro_lowpass", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_soft_lpf_hz, .config.minmax = { 0, 255 } }, - { "gyro_notch_hz", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_hz, .config.minmax = { 0, 500 } }, - { "gyro_notch_cutoff", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_cutoff, .config.minmax = { 1, 500 } }, + { "gyro_notch_hz_1", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_hz_1, .config.minmax = { 0, 1000 } }, + { "gyro_notch_cutoff_1", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_cutoff_1, .config.minmax = { 1, 1000 } }, + { "gyro_notch_hz_2", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_hz_2, .config.minmax = { 0, 1000 } }, + { "gyro_notch_cutoff_2", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_cutoff_2, .config.minmax = { 1, 1000 } }, { "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 } }, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 9a63f909e..d4758acef 100755 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1258,14 +1258,16 @@ static bool processOutCommand(uint8_t cmdMSP) serialize16(masterConfig.motor_pwm_rate); break; case MSP_FILTER_CONFIG : - headSerialReply(13); + headSerialReply(17); serialize8(masterConfig.gyro_soft_lpf_hz); serialize16(currentProfile->pidProfile.dterm_lpf_hz); serialize16(currentProfile->pidProfile.yaw_lpf_hz); - serialize16(masterConfig.gyro_soft_notch_hz); - serialize16(masterConfig.gyro_soft_notch_cutoff); + serialize16(masterConfig.gyro_soft_notch_hz_1); + serialize16(masterConfig.gyro_soft_notch_cutoff_1); serialize16(currentProfile->pidProfile.dterm_notch_hz); serialize16(currentProfile->pidProfile.dterm_notch_cutoff); + serialize16(masterConfig.gyro_soft_notch_hz_2); + serialize16(masterConfig.gyro_soft_notch_cutoff_2); break; case MSP_PID_ADVANCED: headSerialReply(17); @@ -1865,11 +1867,15 @@ static bool processInCommand(void) currentProfile->pidProfile.dterm_lpf_hz = read16(); currentProfile->pidProfile.yaw_lpf_hz = read16(); if (currentPort->dataSize > 5) { - masterConfig.gyro_soft_notch_hz = read16(); - masterConfig.gyro_soft_notch_cutoff = read16(); + masterConfig.gyro_soft_notch_hz_1 = read16(); + masterConfig.gyro_soft_notch_cutoff_1 = read16(); currentProfile->pidProfile.dterm_notch_hz = read16(); currentProfile->pidProfile.dterm_notch_cutoff = read16(); } + if (currentPort->dataSize > 13) { + serialize16(masterConfig.gyro_soft_notch_hz_2); + serialize16(masterConfig.gyro_soft_notch_cutoff_2); + } break; case MSP_SET_PID_ADVANCED: currentProfile->pidProfile.rollPitchItermIgnoreRate = read16(); diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 7c3916856..28cdfb164 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -46,29 +46,38 @@ float gyroADCf[XYZ_AXIS_COUNT]; static int32_t gyroZero[XYZ_AXIS_COUNT] = { 0, 0, 0 }; static const gyroConfig_t *gyroConfig; static biquadFilter_t gyroFilterLPF[XYZ_AXIS_COUNT]; -static biquadFilter_t gyroFilterNotch[XYZ_AXIS_COUNT]; +static biquadFilter_t gyroFilterNotch_1[XYZ_AXIS_COUNT], gyroFilterNotch_2[XYZ_AXIS_COUNT]; static pt1Filter_t gyroFilterPt1[XYZ_AXIS_COUNT]; static uint8_t gyroSoftLpfType; -static uint16_t gyroSoftNotchHz; -static float gyroSoftNotchQ; +static uint16_t gyroSoftNotchHz_1, gyroSoftNotchHz_2; +static float gyroSoftNotchQ_1, gyroSoftNotchQ_2; static uint8_t gyroSoftLpfHz; static uint16_t calibratingG = 0; static float gyroDt; -void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz, uint16_t gyro_soft_notch_hz, uint16_t gyro_soft_notch_cutoff, uint8_t gyro_soft_lpf_type) +void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, + uint8_t gyro_soft_lpf_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, + uint8_t gyro_soft_lpf_type) { gyroConfig = gyroConfigToUse; gyroSoftLpfHz = gyro_soft_lpf_hz; - gyroSoftNotchHz = gyro_soft_notch_hz; + gyroSoftNotchHz_1 = gyro_soft_notch_hz_1; + gyroSoftNotchHz_2 = gyro_soft_notch_hz_2; gyroSoftLpfType = gyro_soft_lpf_type; - gyroSoftNotchQ = filterGetNotchQ(gyro_soft_notch_hz, gyro_soft_notch_cutoff); + gyroSoftNotchQ_1 = filterGetNotchQ(gyro_soft_notch_hz_1, gyro_soft_notch_cutoff_1); + gyroSoftNotchQ_2 = filterGetNotchQ(gyro_soft_notch_hz_2, gyro_soft_notch_cutoff_2); } void gyroInit(void) { if (gyroSoftLpfHz && gyro.targetLooptime) { // Initialisation needs to happen once samplingrate is known for (int axis = 0; axis < 3; axis++) { - biquadFilterInit(&gyroFilterNotch[axis], gyroSoftNotchHz, gyro.targetLooptime, gyroSoftNotchQ, FILTER_NOTCH); + biquadFilterInit(&gyroFilterNotch_1[axis], gyroSoftNotchHz_1, gyro.targetLooptime, gyroSoftNotchQ_1, FILTER_NOTCH); + biquadFilterInit(&gyroFilterNotch_2[axis], gyroSoftNotchHz_2, gyro.targetLooptime, gyroSoftNotchQ_2, FILTER_NOTCH); if (gyroSoftLpfType == FILTER_BIQUAD) biquadFilterInitLPF(&gyroFilterLPF[axis], gyroSoftLpfHz, gyro.targetLooptime); else @@ -183,8 +192,11 @@ void gyroUpdate(void) if (debugMode == DEBUG_NOTCH) debug[axis] = lrintf(gyroADCf[axis]); - if (gyroSoftNotchHz) - gyroADCf[axis] = biquadFilterApply(&gyroFilterNotch[axis], gyroADCf[axis]); + if (gyroSoftNotchHz_1) + gyroADCf[axis] = biquadFilterApply(&gyroFilterNotch_1[axis], gyroADCf[axis]); + + if (gyroSoftNotchHz_2) + gyroADCf[axis] = biquadFilterApply(&gyroFilterNotch_2[axis], gyroADCf[axis]); gyroADC[axis] = lrintf(gyroADCf[axis]); } diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index bae1805e5..11bbc9ce5 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -40,7 +40,13 @@ typedef struct gyroConfig_s { 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. } gyroConfig_t; -void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz, uint16_t gyro_soft_notch_hz, uint16_t gyro_soft_notch_cutoff, uint8_t gyro_soft_lpf_type); +void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, + uint8_t gyro_soft_lpf_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, + uint8_t gyro_soft_lpf_type); void gyroSetCalibrationCycles(void); void gyroInit(void); void gyroUpdate(void);