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

Acc notch and gyro stage2 in MSP

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2018-05-13 10:30:12 +02:00
parent 56ca670bc9
commit 7cee94363c
1 changed files with 30 additions and 1 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1044,6 +1044,20 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
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:
@ -1827,7 +1841,22 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
gyroConfigMutable()->gyro_soft_notch_cutoff_2 = constrain(sbufReadU16(src), 1, 500);
} else
return MSP_RESULT_ERROR;
#endif
#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
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
return MSP_RESULT_ERROR;
#endif
} else
return MSP_RESULT_ERROR;
break;