diff --git a/src/main/java/io/github/dsheirer/dsp/psk/InterpolatingSampleBuffer.java b/src/main/java/io/github/dsheirer/dsp/psk/InterpolatingSampleBuffer.java
index 15adbd9a..f08e6a50 100644
--- a/src/main/java/io/github/dsheirer/dsp/psk/InterpolatingSampleBuffer.java
+++ b/src/main/java/io/github/dsheirer/dsp/psk/InterpolatingSampleBuffer.java
@@ -130,7 +130,7 @@ public class InterpolatingSampleBuffer
      */
     public Complex getPrecedingSample()
     {
-        mPrecedingSample.setValues(mDelayLineInphase[mDelayLinePointer + 4], mDelayLineQuadrature[mDelayLinePointer + 3]);
+        mPrecedingSample.setValues(mDelayLineInphase[mDelayLinePointer + 3], mDelayLineQuadrature[mDelayLinePointer + 3]);
         return mPrecedingSample;
     }
 
diff --git a/src/main/java/io/github/dsheirer/module/decode/p25/P25_C4FMDecoder.java b/src/main/java/io/github/dsheirer/module/decode/p25/P25_C4FMDecoder.java
index 6923bc37..b1969b16 100644
--- a/src/main/java/io/github/dsheirer/module/decode/p25/P25_C4FMDecoder.java
+++ b/src/main/java/io/github/dsheirer/module/decode/p25/P25_C4FMDecoder.java
@@ -20,9 +20,9 @@ import io.github.dsheirer.dsp.filter.FilterFactory;
 import io.github.dsheirer.dsp.filter.design.FilterDesignException;
 import io.github.dsheirer.dsp.filter.fir.FIRFilterSpecification;
 import io.github.dsheirer.dsp.filter.fir.complex.ComplexFIRFilter;
+import io.github.dsheirer.dsp.gain.ComplexFeedForwardGainControl;
 import io.github.dsheirer.dsp.psk.DQPSKDemodulator;
 import io.github.dsheirer.dsp.psk.InterpolatingSampleBuffer;
-import io.github.dsheirer.dsp.psk.InterpolatingSampleBufferInstrumented;
 import io.github.dsheirer.dsp.psk.pll.CostasLoop;
 import io.github.dsheirer.sample.Listener;
 import io.github.dsheirer.sample.complex.ComplexBuffer;
@@ -38,6 +38,7 @@ import java.util.concurrent.ScheduledExecutorService;
 public class P25_C4FMDecoder extends P25Decoder implements IComplexBufferListener, Listener<ComplexBuffer>
 {
     private final static Logger mLog = LoggerFactory.getLogger(P25_C4FMDecoder.class);
+    private ComplexFeedForwardGainControl mAGC = new ComplexFeedForwardGainControl(32);
     protected static final double SYMBOL_RATE = 4800;
     protected InterpolatingSampleBuffer mInterpolatingSampleBuffer;
     protected DQPSKDemodulator mQPSKDemodulator;
@@ -65,7 +66,7 @@ public class P25_C4FMDecoder extends P25Decoder implements IComplexBufferListene
 
         mCostasLoop = new CostasLoop(mSampleRate, SYMBOL_RATE);
 
-        mInterpolatingSampleBuffer = new InterpolatingSampleBufferInstrumented((float)(sampleRate / SYMBOL_RATE));
+        mInterpolatingSampleBuffer = new InterpolatingSampleBuffer((float)(sampleRate / SYMBOL_RATE));
         mQPSKDemodulator = new DQPSKDemodulator(mCostasLoop, mInterpolatingSampleBuffer);
 
         //Message framer can trigger a symbol-inversion correction to the PLL when detected
@@ -77,8 +78,11 @@ public class P25_C4FMDecoder extends P25Decoder implements IComplexBufferListene
     @Override
     public void receive(ComplexBuffer complexBuffer)
     {
-        ComplexBuffer filtered = filter(complexBuffer);
-        mQPSKDemodulator.receive(filtered);
+        ComplexBuffer basebandFiltered = filter(complexBuffer);
+
+        ComplexBuffer gainApplied = mAGC.filter(basebandFiltered);
+
+        mQPSKDemodulator.receive(gainApplied);
     }
 
     protected ComplexBuffer filter(ComplexBuffer complexBuffer)