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:
parent
d451a755bd
commit
30ed4d10d9
1 changed files with 67 additions and 0 deletions
|
@ -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();
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue