Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-22 07:45:29 +03:00
Code Issues Wiki Activity

Add dyn notch to MSP

Browse source
This commit is contained in:
IvoFPV 2019-08-08 13:47:26 +02:00
parent 09c98513b8
commit 2ff2ceec28
1 changed files with 26 additions and 1 deletions
Download patch file Download diff file Expand all files Collapse all files

27
src/main/msp/msp.c
View file

@ -1453,7 +1453,18 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
sbufWriteU16(dst, 0); sbufWriteU16(dst, 0);
sbufWriteU16(dst, 0); sbufWriteU16(dst, 0);
#endif #endif
#if defined(USE_GYRO_DATA_ANALYSE)
// Added in MSP API 1.42
sbufWriteU8(dst, gyroConfig()->dyn_notch_range);
sbufWriteU8(dst, gyroConfig()->dyn_notch_width_percent);
sbufWriteU16(dst, gyroConfig()->dyn_notch_q);
sbufWriteU16(dst, gyroConfig()->dyn_notch_min_hz);
#else
sbufWriteU8(dst, 0);
sbufWriteU8(dst, 0);
sbufWriteU16(dst, 0);
sbufWriteU16(dst, 0);
#endif
break; break;
case MSP_PID_ADVANCED: case MSP_PID_ADVANCED:
sbufWriteU16(dst, 0); sbufWriteU16(dst, 0);
@ -2120,6 +2131,20 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
sbufReadU16(src); sbufReadU16(src);
#endif #endif
} }
if (sbufBytesRemaining(src) >= 6) {
// Added in MSP API 1.42
#if defined(USE_GYRO_DATA_ANALYSE)
gyroConfigMutable()->dyn_notch_range = sbufReadU8(src);
gyroConfigMutable()->dyn_notch_width_percent = sbufReadU8(src);
gyroConfigMutable()->dyn_notch_q = sbufReadU16(src);
gyroConfigMutable()->dyn_notch_min_hz = sbufReadU16(src);
#else
sbufReadU8(src);
sbufReadU8(src);
sbufReadU16(src);
sbufReadU16(src);
#endif
}
// reinitialize the gyro filters with the new values // reinitialize the gyro filters with the new values
validateAndFixGyroConfig(); validateAndFixGyroConfig();