From 5dc8ff22e09759b533fca8b6dc9a1c1fdced98d5 Mon Sep 17 00:00:00 2001 From: Petri Mattila Date: Tue, 30 Apr 2024 09:48:20 +0100 Subject: [PATCH] WIP --- src/main/flight/pid.c | 12 +++++------- src/main/flight/pid.h | 3 ++- 2 files changed, 7 insertions(+), 8 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 0320e4e95c..227562353a 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -220,7 +220,8 @@ void INIT_CODE pidInitProfile(const pidProfile_t *pidProfile) pt1FilterInit(&pid.precomp.collFilter, 100.0f / constrainf(pidProfile->yaw_collective_dynamic_decay, 1, 250), pid.freq); // Yaw precomp lowpass filter - lowpassFilterInit(&pid.precomp.yawFilter, pidProfile->yaw_precomp_filter_type, pidProfile->yaw_precomp_cutoff, pid.freq, 0); + lowpassFilterInit(&pid.precomp.yawCollFilter, pidProfile->yaw_precomp_filter_type, pidProfile->yaw_precomp_cutoff, pid.freq, 0); + lowpassFilterInit(&pid.precomp.yawCyclFilter, pidProfile->yaw_precomp_filter_type, pidProfile->yaw_precomp_cutoff, pid.freq, 0); // Tail/yaw precomp pid.precomp.yawCyclicFFGain = pidProfile->yaw_cyclic_ff_gain / 100.0f; @@ -399,17 +400,14 @@ static void pidApplyPrecomp(void) //// Collective-to-Yaw Precomp // Collective components - const float yawCollectiveFF = fabsf(collectiveDeflection) * pid.precomp.yawCollectiveFFGain; + const float yawCollectiveFF = fabsf(filterApply(&pid.precomp.yawCollFilter, collectiveDeflection)) * pid.precomp.yawCollectiveFFGain; const float yawCollectiveHF = fabsf(collectiveHF) * pid.precomp.yawCollectiveDynamicGain; // Cyclic component - float yawCyclicFF = fabsf(cyclicDeflection) * pid.precomp.yawCyclicFFGain; + float yawCyclicFF = fabsf(filterApply(&pid.precomp.yawCyclFilter, cyclicDeflection)) * pid.precomp.yawCyclicFFGain; // Calculate total precompensation - float yawPrecomp = yawCollectiveFF + yawCollectiveHF + yawCyclicFF; - - // Lowpass filter - yawPrecomp = filterApply(&pid.precomp.yawFilter, yawPrecomp) * masterGain; + float yawPrecomp = (yawCollectiveFF + yawCollectiveHF + yawCyclicFF) * masterGain; // Add to YAW feedforward pid.data[FD_YAW].F += yawPrecomp; diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 4ed0ad6b7a..d7f2c8fb75 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -79,7 +79,8 @@ typedef struct { typedef struct { - filter_t yawFilter; + filter_t yawCollFilter; + filter_t yawCyclFilter; pt1Filter_t collFilter;