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

MSP_FILTER_CONFIG and MSP_ADVANCED_TUNING

This commit is contained in:
Pawel Spychalski (DzikuVx) 2016-10-18 13:42:31 +02:00
parent d451a755bd
commit 30ed4d10d9

View file

@ -1032,6 +1032,38 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, sbuf_t *src, msp
sbufWriteU8(dst, masterConfig.gyroSync);
break;
case MSP_FILTER_CONFIG :
sbufWriteU8(dst, currentProfile->pidProfile.gyro_soft_lpf_hz);
sbufWriteU16(dst, currentProfile->pidProfile.dterm_lpf_hz);
sbufWriteU16(dst, currentProfile->pidProfile.yaw_lpf_hz);
sbufWriteU16(dst, 1); //masterConfig.gyro_soft_notch_hz_1
sbufWriteU16(dst, 1); //BF: masterConfig.gyro_soft_notch_cutoff_1
sbufWriteU16(dst, 1); //BF: currentProfile->pidProfile.dterm_notch_hz
sbufWriteU16(dst, 1); //currentProfile->pidProfile.dterm_notch_cutoff
sbufWriteU16(dst, 1); //BF: masterConfig.gyro_soft_notch_hz_2
sbufWriteU16(dst, 1); //BF: masterConfig.gyro_soft_notch_cutoff_2
break;
case MSP_ADVANCED_TUNING:
sbufWriteU16(dst, currentProfile->pidProfile.rollPitchItermIgnoreRate);
sbufWriteU16(dst, currentProfile->pidProfile.yawItermIgnoreRate);
sbufWriteU16(dst, currentProfile->pidProfile.yaw_p_limit);
sbufWriteU8(dst, 0); //BF: currentProfile->pidProfile.deltaMethod
sbufWriteU8(dst, 0); //BF: currentProfile->pidProfile.vbatPidCompensation
sbufWriteU8(dst, 0); //BF: currentProfile->pidProfile.setpointRelaxRatio
sbufWriteU8(dst, 0); //BF: currentProfile->pidProfile.dtermSetpointWeight
sbufWriteU8(dst, 0); // reserved
sbufWriteU8(dst, 0); // reserved
sbufWriteU8(dst, 0); //BF: currentProfile->pidProfile.itermThrottleGain
/*
* To keep compatibility on MSP level with Betaflight, axis axisAccelerationLimitYaw
* limit will be sent and received in [dps / 1000]
*/
sbufWriteU16(dst, constrain(currentProfile->pidProfile.axisAccelerationLimitRollPitch / 1000, 0, 65535));
sbufWriteU16(dst, constrain(currentProfile->pidProfile.axisAccelerationLimitYaw / 1000, 0, 65535));
break;
case MSP_REBOOT:
if (mspPostProcessFn) {
*mspPostProcessFn = mspRebootFn;
@ -1321,6 +1353,41 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
masterConfig.gyroSync = sbufReadU8(src);
break;
case MSP_SET_FILTER_CONFIG :
currentProfile->pidProfile.gyro_soft_lpf_hz = sbufReadU8(src);
currentProfile->pidProfile.dterm_lpf_hz = constrain(sbufReadU16(src), 0, 255);
currentProfile->pidProfile.yaw_lpf_hz = constrain(sbufReadU16(src), 0, 255);
//BF: masterConfig.gyro_soft_notch_hz_1 = read16();
//BF: masterConfig.gyro_soft_notch_cutoff_1 = read16();
//BF: currentProfile->pidProfile.dterm_notch_hz = read16();
//BF: currentProfile->pidProfile.dterm_notch_cutoff = read16();
//BF: masterConfig.gyro_soft_notch_hz_2 = read16();
//BF: masterConfig.gyro_soft_notch_cutoff_2 = read16();
break;
case MSP_SET_ADVANCED_TUNING:
currentProfile->pidProfile.rollPitchItermIgnoreRate = sbufReadU16(src);
currentProfile->pidProfile.yawItermIgnoreRate = sbufReadU16(src);
currentProfile->pidProfile.yaw_p_limit = sbufReadU16(src);
sbufReadU8(src); //BF: currentProfile->pidProfile.deltaMethod
sbufReadU8(src); //BF: currentProfile->pidProfile.vbatPidCompensation
sbufReadU8(src); //BF: currentProfile->pidProfile.setpointRelaxRatio
sbufReadU8(src); //BF: currentProfile->pidProfile.dtermSetpointWeight
sbufReadU8(src); // reserved
sbufReadU8(src); // reserved
sbufReadU8(src); //BF: currentProfile->pidProfile.itermThrottleGain
/*
* To keep compatibility on MSP level with Betaflight, axis axisAccelerationLimitYaw
* limit will be sent and received in [dps / 1000]
*/
currentProfile->pidProfile.axisAccelerationLimitRollPitch = sbufReadU16(src) * 1000;
currentProfile->pidProfile.axisAccelerationLimitYaw = sbufReadU16(src) * 1000;
break;
case MSP_RESET_CONF:
if (!ARMING_FLAG(ARMED)) {
resetEEPROM();