Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-24 00:35:34 +03:00
Code Issues Wiki Activity

Merge remote-tracking branch 'origin/development' into dzikuvx-yaw-handling-cleanup

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2019-11-30 09:39:17 +01:00
commit f8717e19d5
18 changed files with 245 additions and 332 deletions
Download patch file Download diff file Expand all files Collapse all files

49
src/main/fc/fc_msp.c
View file

@ -1101,43 +1101,19 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
sbufWriteU8(dst, gyroConfig()->gyro_soft_lpf_hz);
sbufWriteU16(dst, pidProfile()->dterm_lpf_hz);
sbufWriteU16(dst, pidProfile()->yaw_lpf_hz);
#ifdef USE_GYRO_NOTCH_1
sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_hz_1); //masterConfig.gyro_soft_notch_hz_1
sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_cutoff_1); //BF: masterConfig.gyro_soft_notch_cutoff_1
#else
sbufWriteU16(dst, 0); //masterConfig.gyro_soft_notch_hz_1
sbufWriteU16(dst, 1); //BF: masterConfig.gyro_soft_notch_cutoff_1
#endif
#ifdef USE_DTERM_NOTCH
sbufWriteU16(dst, pidProfile()->dterm_soft_notch_hz); //BF: pidProfile()->dterm_notch_hz
sbufWriteU16(dst, pidProfile()->dterm_soft_notch_cutoff); //pidProfile()->dterm_notch_cutoff
#else
sbufWriteU16(dst, 0); //BF: pidProfile()->dterm_notch_hz
sbufWriteU16(dst, 1); //pidProfile()->dterm_notch_cutoff
#endif
#ifdef USE_GYRO_NOTCH_2
sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_hz_2); //BF: masterConfig.gyro_soft_notch_hz_2
sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_cutoff_2); //BF: masterConfig.gyro_soft_notch_cutoff_2
#else
sbufWriteU16(dst, 0); //BF: masterConfig.gyro_soft_notch_hz_2
sbufWriteU16(dst, 1); //BF: masterConfig.gyro_soft_notch_cutoff_2
#endif
#ifdef USE_ACC_NOTCH
sbufWriteU16(dst, accelerometerConfig()->acc_notch_hz);
sbufWriteU16(dst, accelerometerConfig()->acc_notch_cutoff);
#else
sbufWriteU16(dst, 0);
sbufWriteU16(dst, 1);
#endif
#ifdef USE_GYRO_BIQUAD_RC_FIR2
sbufWriteU16(dst, gyroConfig()->gyro_stage2_lowpass_hz);
#else
sbufWriteU16(dst, 0);
#endif
break;
case MSP_PID_ADVANCED:
@ -2017,43 +1993,38 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
gyroConfigMutable()->gyro_soft_lpf_hz = sbufReadU8(src);
pidProfileMutable()->dterm_lpf_hz = constrain(sbufReadU16(src), 0, 255);
pidProfileMutable()->yaw_lpf_hz = constrain(sbufReadU16(src), 0, 255);
#ifdef USE_GYRO_NOTCH_1
if (dataSize >= 9) {
gyroConfigMutable()->gyro_soft_notch_hz_1 = constrain(sbufReadU16(src), 0, 500);
gyroConfigMutable()->gyro_soft_notch_cutoff_1 = constrain(sbufReadU16(src), 1, 500);
} else
} else {
return MSP_RESULT_ERROR;
#endif
#ifdef USE_DTERM_NOTCH
}
if (dataSize >= 13) {
pidProfileMutable()->dterm_soft_notch_hz = constrain(sbufReadU16(src), 0, 500);
pidProfileMutable()->dterm_soft_notch_cutoff = constrain(sbufReadU16(src), 1, 500);
pidInitFilters();
} else
} else {
return MSP_RESULT_ERROR;
#endif
#ifdef USE_GYRO_NOTCH_2
}
if (dataSize >= 17) {
gyroConfigMutable()->gyro_soft_notch_hz_2 = constrain(sbufReadU16(src), 0, 500);
gyroConfigMutable()->gyro_soft_notch_cutoff_2 = constrain(sbufReadU16(src), 1, 500);
} else
} else {
return MSP_RESULT_ERROR;
#endif
}
#ifdef USE_ACC_NOTCH
if (dataSize >= 21) {
accelerometerConfigMutable()->acc_notch_hz = constrain(sbufReadU16(src), 0, 255);
accelerometerConfigMutable()->acc_notch_cutoff = constrain(sbufReadU16(src), 1, 255);
} else
} else {
return MSP_RESULT_ERROR;
#endif
}
#ifdef USE_GYRO_BIQUAD_RC_FIR2
if (dataSize >= 22) {
gyroConfigMutable()->gyro_stage2_lowpass_hz = constrain(sbufReadU16(src), 0, 500);
} else
} else {
return MSP_RESULT_ERROR;
#endif
}
} else
return MSP_RESULT_ERROR;
break;