From 52f2f61a9a00a681dce24050080790b8be1f6b0a Mon Sep 17 00:00:00 2001
From: KubaPro010 <kuba.pro010@gmail.com>
Date: Tue, 8 Jul 2025 19:54:15 +0200
Subject: [PATCH] tilt correction? (yes, ai wrote it, you hate me because of
 that? find a tilt filter yourself then that you can copy the code of)

---
 filter/iir.c | 45 +++++++++++++++++++++++++++++++++++++++++++++
 filter/iir.h | 11 +++++++++++
 src/fm95.c   |  9 ++++++++-
 3 files changed, 64 insertions(+), 1 deletion(-)

diff --git a/filter/iir.c b/filter/iir.c
index fb19f04..7a022d5 100644
--- a/filter/iir.c
+++ b/filter/iir.c
@@ -13,4 +13,49 @@ inline float apply_preemphasis(ResistorCapacitor *filter, float sample) {
 	float out = (sample - filter->alpha * filter->prev_sample) * filter->gain;
 	filter->prev_sample = sample;
 	return out;
+}
+
+void tilt_init(TiltCorrectionFilter* filter, float correction_strength) {
+    // This filter is a first-order IIR low-shelf filter.
+    // The difference equation is: y[n] = b0*x[n] + b1*x[n-1] - a1*y[n-1]
+    // We simplify it to y[n] = x[n] - a1*y[n-1] which acts as a leaky integrator.
+    
+    // The "correction_strength" is our leaky factor. It is the pole of the filter.
+    // A value close to 1.0 places the pole very close to the unit circle,
+    // providing a large boost to low frequencies (and DC).
+    
+    if (correction_strength >= 1.0f) {
+        correction_strength = 0.99999f; // Prevent instability
+    }
+
+    filter->b0 = 1.0f;
+    filter->b1 = 0.0f;
+    filter->a1 = -correction_strength; // The feedback coefficient
+
+    // Reset filter state
+    filter->x_prev = 0.0f;
+    filter->y_prev = 0.0f;
+}
+
+float tilt(TiltCorrectionFilter* filter, float input_sample) {
+    // Apply the difference equation: y[n] = b0*x[n] + b1*x[n-1] - a1*y[n-1]
+    float output_sample = filter->b0 * input_sample + filter->b1 * filter->x_prev - filter->a1 * filter->y_prev;
+
+    // Important: Prevent output from running away due to DC offset accumulation
+    // This is a simple guard. If the filter becomes unstable or the output
+    // grows too large, it gets reset. For square waves, the absolute value of the
+    // output should not significantly exceed the absolute value of the input.
+    if (fabsf(output_sample) > 2.0f * fabsf(input_sample) && fabsf(input_sample) > 0.001f) {
+       // This condition indicates the filter state might be diverging. Resetting it.
+       // You may need to adjust the '2.0f' factor based on your signal.
+       filter->y_prev = 0; 
+       output_sample = input_sample;
+    }
+
+
+    // Update the state for the next iteration
+    filter->x_prev = input_sample;
+    filter->y_prev = output_sample;
+    
+    return output_sample;
 }
\ No newline at end of file
diff --git a/filter/iir.h b/filter/iir.h
index 9005513..744bb46 100644
--- a/filter/iir.h
+++ b/filter/iir.h
@@ -12,3 +12,14 @@ typedef struct
 
 void init_preemphasis(ResistorCapacitor *filter, float tau, float sample_rate, float ref_freq);
 float apply_preemphasis(ResistorCapacitor *filter, float sample);
+
+typedef struct {
+    float b0, b1;
+    float a1;
+
+    float x_prev;
+    float y_prev;
+} TiltCorrectionFilter;
+
+void tilt_init(TiltCorrectionFilter *filter, float correction_strength);
+float tilt(TiltCorrectionFilter *filter, float input_sample);
diff --git a/src/fm95.c b/src/fm95.c
index ab77c67..5653c62 100644
--- a/src/fm95.c
+++ b/src/fm95.c
@@ -65,6 +65,7 @@ typedef struct
 
 	float clipper_threshold;
 	float preemphasis;
+	float tilt;
 	uint8_t calibration;
 	float mpx_power;
 	float mpx_deviation;
@@ -168,6 +169,9 @@ int run_fm95(const FM95_Config config, FM95_Runtime* runtime) {
 	MPXPowerMeasurement power;
 	init_modulation_power_measure(&power, config.sample_rate);
 
+	TiltCorrectionFilter tilter;
+	tilt_init(&tilter, config.tilt);
+
 	StereoEncoder stencode;
 	init_stereo_encoder(&stencode, 4.0f, &osc, (config.stereo == 2), config.volumes.mono, config.volumes.pilot, config.volumes.stereo);
 
@@ -248,7 +252,7 @@ int run_fm95(const FM95_Config config, FM95_Runtime* runtime) {
 
 			mpx *= bs412_audio_gain;
 			
-			output[i] = hard_clip((mpx_in[i]+mpx)*config.master_volume, 1.0); // Ensure peak deviation of 75 khz, assuming we're calibrated correctly (lower)
+			output[i] = hard_clip(tilt(&tilter, (mpx_in[i]+mpx))*config.master_volume, 1.0); // Ensure peak deviation of 75 khz, assuming we're calibrated correctly (lower)
 			advance_oscillator(&osc);
 		}
 
@@ -365,6 +369,8 @@ static int config_handler(void* user, const char* section, const char* name, con
 		pconfig->volumes.rds = strtof(value, NULL);
 	} else if(MATCH("volumes", "rds_step")) {
 		pconfig->volumes.rds_step = strtof(value, NULL);
+	} else if(MATCH("fm95", "tilt")) {
+		pconfig->tilt = strtof(value, NULL);
 	} else {
         return 0; // Unknown section/name
     }
@@ -452,6 +458,7 @@ int main(int argc, char **argv) {
 
 		.clipper_threshold = DEFAULT_CLIPPER_THRESHOLD,
 		.preemphasis = DEFAULT_PREEMPHASIS_TAU,
+		.tilt = 0,
 		.calibration = 0,
 		.mpx_power = DEFAULT_MPX_POWER,
 		.mpx_deviation = DEFAULT_MPX_DEVIATION,