diff --git a/device.h b/device.h
index af5e707..5af98e2 100755
--- a/device.h
+++ b/device.h
@@ -14,6 +14,7 @@
 
 // Sampling & timer setup
 #define CONFIG_SAMPLERATE 19200UL
+//#define CONFIG_SAMPLERATE 9600UL
 
 // Serial settings
 #define BAUD 115200
diff --git a/hardware/AFSK.c b/hardware/AFSK.c
index dee9c83..01bf84f 100755
--- a/hardware/AFSK.c
+++ b/hardware/AFSK.c
@@ -376,44 +376,67 @@ void AFSK_adc_isr(Afsk *afsk, int8_t currentSample) {
     afsk->iirX[0] = afsk->iirX[1];
 
     #if CONFIG_SAMPLERATE == 9600
-        #if FILTER_CUTOFF == 600
-            afsk->iirX[1] = ((int8_t)fifo_pop(&afsk->delayFifo) * currentSample) >> 2;
-            // The above is a simplification of:
-            // afsk->iirX[1] = ((int8_t)fifo_pop(&afsk->delayFifo) * currentSample) / 3.558147322;        
-        #else
-            #error Unsupported filter cutoff!
-        #endif
-    #elif CONFIG_SAMPLERATE == 19200
-        #if FILTER_CUTOFF == 600
-            afsk->iirX[1] = ((int8_t)fifo_pop(&afsk->delayFifo) * currentSample) / 6;
-        #else
-            #error Unsupported filter cutoff!
-        #endif
-    #else
-        #error Unsupported samplerate!
-    #endif
+        #if FILTER_CUTOFF == 500
+            #define IIR_GAIN 4 // Really 4.082041675
+            #define IIR_POLE 2 // Really Y[0] * 0.5100490981
+            afsk->iirX[1] = ((int8_t)fifo_pop(&afsk->delayFifo) * currentSample) / IIR_GAIN;
+            afsk->iirY[0] = afsk->iirY[1];
+            afsk->iirY[1] = afsk->iirX[0] + afsk->iirX[1] + (afsk->iirY[0] / 2);
 
-    afsk->iirY[0] = afsk->iirY[1];
-    
-    #if CONFIG_SAMPLERATE == 9600
-        #if FILTER_CUTOFF == 600
-            afsk->iirY[1] = afsk->iirX[0] + afsk->iirX[1] + (afsk->iirY[0] >> 1);
-            // The above is a simplification of:
-            // afsk->iirY[1] = afsk->iirX[0] + afsk->iirX[1] + (afsk->iirY[0] * 0.4379097269);
         #else
             #error Unsupported filter cutoff!
         #endif
+
     #elif CONFIG_SAMPLERATE == 19200
-        #if FILTER_CUTOFF == 600
-            afsk->iirY[1] = afsk->iirX[0] + afsk->iirX[1] + (afsk->iirY[0] / 2);            
+
+        #if FILTER_CUTOFF == 150
+            #define IIR_GAIN 2 // Really 2.172813446e
+            #define IIR_POLE 2 // Really Y[0] * 0.9079534415
+            afsk->iirX[1] = ((int8_t)fifo_pop(&afsk->delayFifo) * currentSample) / IIR_GAIN;
+            afsk->iirY[0] = afsk->iirY[1];
+            afsk->iirY[1] = afsk->iirX[0] + afsk->iirX[1] + (afsk->iirY[0] / IIR_POLE); 
+
+        #elif FILTER_CUTOFF == 500
+            #define IIR_GAIN 7 // Really 5.006847792
+            #define IIR_POLE 2 // Really Y[0] * 0.6005470741
+            afsk->iirX[1] = ((int8_t)fifo_pop(&afsk->delayFifo) * currentSample) / IIR_GAIN;
+            afsk->iirY[0] = afsk->iirY[1];
+            afsk->iirY[1] = afsk->iirX[0] + afsk->iirX[1] + (afsk->iirY[0] / IIR_POLE); 
+
+        #elif FILTER_CUTOFF == 600
+            #define IIR_GAIN 6 // Really 6.166411713
+            #define IIR_POLE 2 // Really Y[0] * 0.6756622663
+            afsk->iirX[1] = ((int8_t)fifo_pop(&afsk->delayFifo) * currentSample) / IIR_GAIN;
+            afsk->iirY[0] = afsk->iirY[1];
+            afsk->iirY[1] = afsk->iirX[0] + afsk->iirX[1] + (afsk->iirY[0] / IIR_POLE); 
+
+        #elif FILTER_CUTOFF == 772
+            #define IIR_GAIN 5 // Really 5.006847792
+            #define IIR_POLE 2 // Really Y[0] * 0.6005470741
+            afsk->iirX[1] = ((int8_t)fifo_pop(&afsk->delayFifo) * currentSample) / IIR_GAIN;
+            afsk->iirY[0] = afsk->iirY[1];
+            afsk->iirY[1] = afsk->iirX[0] + afsk->iirX[1] + (afsk->iirY[0] / IIR_POLE);  
+
+        #elif FILTER_CUTOFF == 1000
+            #define IIR_GAIN 4 // Really 4.082041675
+            #define IIR_POLE 2 // Really Y[0] * 0.5100490981
+            afsk->iirX[1] = ((int8_t)fifo_pop(&afsk->delayFifo) * currentSample) / IIR_GAIN;
+            afsk->iirY[0] = afsk->iirY[1];
+            afsk->iirY[1] = afsk->iirX[0] + afsk->iirX[1] + (afsk->iirY[0] / IIR_POLE);
+
+        #elif FILTER_CUTOFF == 1400
+            #define IIR_GAIN 3 // Really 3.182326364
+            #define IIR_POLE 3 // Really Y[0] * 0.3715289474
+            afsk->iirX[1] = ((int8_t)fifo_pop(&afsk->delayFifo) * currentSample) / IIR_GAIN;
+            afsk->iirY[0] = afsk->iirY[1];
+            afsk->iirY[1] = afsk->iirX[0] + afsk->iirX[1] + (afsk->iirY[0] / IIR_POLE);    
+
         #else
             #error Unsupported filter cutoff!
         #endif
     #else
-        #error Unsupported samplerate!
+        #error No filters defined for specified samplerate!
     #endif
-    
-    //int8_t freq_disc = (int8_t)fifo_pop(&afsk->delayFifo) * currentSample;
 
     // We put the sampled bit in a delay-line:
     // First we bitshift everything 1 left
@@ -551,16 +574,13 @@ void AFSK_adc_isr(Afsk *afsk, int8_t currentSample) {
 
 ISR(ADC_vect) {
     TIFR1 = _BV(ICF1);
-
-    //DAC_PORT ^= 0xFF;
-    //DAC_PORT = ADCH;
-
-    AFSK_adc_isr(AFSK_modem, (ADCH - 128));
-
+    
     if (hw_afsk_dac_isr) {
         DAC_PORT = AFSK_dac_isr(AFSK_modem);
         LED_TX_ON();
     } else {
+        // TODO: Enable full duplex if possible
+        AFSK_adc_isr(AFSK_modem, (ADCH - 128));
         DAC_PORT = 127;
         LED_TX_OFF();
     }
@@ -569,6 +589,10 @@ ISR(ADC_vect) {
 
     /*
     // TODO: Remove these debug sample collection functions
+
+    //DAC_PORT ^= 0xFF;
+    //DAC_PORT = ADCH;
+
     if (capturedsamples == SAMPLES_TO_CAPTURE) {
         printf("--- Dumping samples ---");
         for (ticks_t i = 0; i < SAMPLES_TO_CAPTURE; i++) {
diff --git a/hardware/AFSK.h b/hardware/AFSK.h
index b6698f5..312e701 100755
--- a/hardware/AFSK.h
+++ b/hardware/AFSK.h
@@ -11,7 +11,7 @@
 #include "protocol/HDLC.h"
 
 #define SIN_LEN 512
-static const uint8_t sin_table[] PROGMEM =
+static const uint8_t sine_table[] =
 {
     128, 129, 131, 132, 134, 135, 137, 138, 140, 142, 143, 145, 146, 148, 149, 151,
     152, 154, 155, 157, 158, 160, 162, 163, 165, 166, 167, 169, 170, 172, 173, 175,
@@ -26,7 +26,7 @@ static const uint8_t sin_table[] PROGMEM =
 inline static uint8_t sinSample(uint16_t i) {
     uint16_t newI = i % (SIN_LEN/2);
     newI = (newI >= (SIN_LEN/4)) ? (SIN_LEN/2 - newI -1) : newI;
-    uint8_t sine = pgm_read_byte(&sin_table[newI]);
+    uint8_t sine = sine_table[newI];
     return (i >= (SIN_LEN/2)) ? (255 - sine) : sine;
 }
 
@@ -34,12 +34,8 @@ inline static uint8_t sinSample(uint16_t i) {
 #define SWITCH_TONE(inc)  (((inc) == MARK_INC) ? SPACE_INC : MARK_INC)
 #define BITS_DIFFER(bits1, bits2) (((bits1)^(bits2)) & 0x01)
 #define TRANSITION_FOUND(bits) BITS_DIFFER((bits), (bits) >> 1)
-
-// TODO: Maybe revert to only looking at two samples
 #define DUAL_XOR(bits1, bits2) ((((bits1)^(bits2)) & 0x03) == 0x03)
 #define QUAD_XOR(bits1, bits2) ((((bits1)^(bits2)) & 0x0F) == 0x0F)
-#define SIGNAL_TRANSITIONED(bits) QUAD_XOR((bits), (bits) >> 4)
-// #define SIGNAL_TRANSITIONED(bits) DUAL_XOR((bits), (bits) >> 2)
 
 #define CPU_FREQ F_CPU
 
@@ -47,22 +43,37 @@ inline static uint8_t sinSample(uint16_t i) {
 #define CONFIG_AFSK_TX_BUFLEN CONFIG_SAMPLERATE/150
 #define CONFIG_AFSK_RXTIMEOUT 0
 #define CONFIG_AFSK_TXWAIT    0UL
-#define CONFIG_AFSK_PREAMBLE_LEN 350UL
-#define CONFIG_AFSK_TRAILER_LEN 50UL
+#define CONFIG_AFSK_PREAMBLE_LEN 450UL
+#define CONFIG_AFSK_TRAILER_LEN 10UL
 #define BIT_STUFF_LEN 5
 
 #define BITRATE    1200
 #define SAMPLESPERBIT (CONFIG_SAMPLERATE / BITRATE)
 #define TICKS_BETWEEN_SAMPLES ((((CPU_FREQ+FREQUENCY_CORRECTION)) / CONFIG_SAMPLERATE) - 1)
 
-// TODO: Calculate based on sample rate [Done?]
-#define PHASE_BITS   8                              // 8    // Sub-sample phase counter resolution
-#define PHASE_INC    1                              // 1    // Nudge by above resolution for each adjustment
+// TODO: Maybe revert to only looking at two samples
+
+#if BITRATE == 1200
+    #define SIGNAL_TRANSITIONED(bits) QUAD_XOR((bits), (bits) >> 4)
+#elif BITRATE == 2400
+    #define SIGNAL_TRANSITIONED(bits) DUAL_XOR((bits), (bits) >> 2)
+#endif
+
+// TODO: Calculate based on sample rate [Done?]
+#define PHASE_BITS   8                              // Sub-sample phase counter resolution
+#define PHASE_INC    1                              // Nudge by above resolution for each adjustment
+
+#define PHASE_MAX    (SAMPLESPERBIT * PHASE_BITS)   // Size of our phase counter
 
-#define PHASE_MAX    (SAMPLESPERBIT * PHASE_BITS)   // 128  // Size of our phase counter
 // TODO: Test which target is best in real world
-#define PHASE_THRESHOLD  (PHASE_MAX / 2)+3*PHASE_BITS  // Target transition point of our phase window
-//#define PHASE_THRESHOLD  (PHASE_MAX / 2)            // 64   // Target transition point of our phase window
+// For 1200, this seems a little better
+#if BITRATE == 1200
+    #define PHASE_THRESHOLD  (PHASE_MAX / 2)+3*PHASE_BITS  // Target transition point of our phase window
+    //#define PHASE_THRESHOLD  (PHASE_MAX / 2)            // 64   // Target transition point of our phase window
+#elif BITRATE == 2400
+    #define PHASE_THRESHOLD  (PHASE_MAX / 2)+14  // Target transition point of our phase window
+#endif
+
 
 #define DCD_TIMEOUT_SAMPLES CONFIG_SAMPLERATE/100
 #define DCD_MIN_COUNT CONFIG_SAMPLERATE/1600
@@ -72,6 +83,16 @@ inline static uint8_t sinSample(uint16_t i) {
     #define FILTER_CUTOFF 600
     #define MARK_FREQ  1200
     #define SPACE_FREQ 2200
+#elif BITRATE == 2400
+    #define FILTER_CUTOFF 772
+    // #define MARK_FREQ  2165
+    // #define SPACE_FREQ 3970
+    #define MARK_FREQ  2200
+    #define SPACE_FREQ 4000
+#elif BITRATE == 300
+    #define FILTER_CUTOFF 600
+    #define MARK_FREQ  1600
+    #define SPACE_FREQ 1800
 #else
     #error Unsupported bitrate!
 #endif
@@ -117,7 +138,12 @@ typedef struct Afsk
 
     // Demodulation values
     FIFOBuffer delayFifo;                   // Delayed FIFO for frequency discrimination
-    int8_t delayBuf[SAMPLESPERBIT / 2 + 1]; // Actual data storage for said FIFO
+    #if BITRATE == 1200
+        int8_t delayBuf[SAMPLESPERBIT / 2 + 1]; // Actual data storage for said FIFO
+        //int8_t delayBuf[3 + 1]; // Actual data storage for said FIFO
+    #elif BITRATE == 2400
+        int8_t delayBuf[7 + 1]; // Actual data storage for said FIFO
+    #endif
 
     FIFOBuffer rxFifo;                      // FIFO for received data
     uint8_t rxBuf[CONFIG_AFSK_RX_BUFLEN];   // Actual data storage for said FIFO
@@ -128,7 +154,9 @@ typedef struct Afsk
     #if SAMPLESPERBIT < 17
         uint16_t sampledBits;               // Bits sampled by the demodulator (at ADC speed)
     #else
-        #error Not enough space in sampledBits variable!
+        // TODO: Enable error and set up correct size buffers
+        uint16_t sampledBits;
+        //#error Not enough space in sampledBits variable!
     #endif
     int16_t currentPhase;                    // Current phase of the demodulator
     uint8_t actualBits;                     // Actual found bits at correct bitrate
diff --git a/protocol/KISS.c b/protocol/KISS.c
index cc7f68c..d3511a9 100755
--- a/protocol/KISS.c
+++ b/protocol/KISS.c
@@ -27,7 +27,12 @@ void kiss_init(AX25Ctx *ax25, Afsk *afsk, Serial *ser) {
     FLOWCONTROL = false;
 }
 
+// TODO: Revert to actual data
+size_t decodes = 0;
 void kiss_messageCallback(AX25Ctx *ctx) {
+    decodes++;
+    printf("%d\r\n", decodes);
+    /*
     fputc(FEND, &serial->uart0);
     fputc(0x00, &serial->uart0);
     for (unsigned i = 0; i < ctx->frame_len-2; i++) {
@@ -42,7 +47,7 @@ void kiss_messageCallback(AX25Ctx *ctx) {
             fputc(b, &serial->uart0);
         }
     }
-    fputc(FEND, &serial->uart0);
+    fputc(FEND, &serial->uart0);*/
 }
 
 void kiss_csma(AX25Ctx *ctx, uint8_t *buf, size_t len) {