WIP

rotorflight · Apr 30, 2024 · 5dc8ff2 · 5dc8ff2
1 parent b7e3783
commit 5dc8ff2
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 8 deletions.
diff --git 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
@@ -79,7 +79,8 @@ typedef struct {
 
 typedef struct {
 
-    filter_t yawFilter;
+    filter_t yawCollFilter;
+    filter_t yawCyclFilter;
 
     pt1Filter_t collFilter;