Optimized RF settings. Create basic link from SPP code to TNC

main/afsk_demodulator.c

@@ -0,0 +1,78 @@
+/*
+ * afsk_demodulator.c
+ *
+ *  Created on: Sep 10, 2019
+ *      Author: curiousmuch
+ */
+
+#include "stdio.h"
+#include "stdint.h"
+#include "math.h"
+#include "afsk_demodulator.h"
+
+int8_t window[WINDOW_SIZE];
+//int32_t lpf_window[WINDOW_SIZE];
+
+double goertzelFilter(int8_t samples[], double freq, unsigned int N)
+{
+    double s_prev = 0.0;
+    double s_prev2 = 0.0;
+    double coeff,normalizedfreq,power,s;
+    unsigned int i;
+    normalizedfreq = freq / SAMPLEFREQUENCY;
+    coeff = 2*cos(2*M_PI*normalizedfreq);
+    for (i=0; i<N; i++) {
+        s = samples[i] + coeff * s_prev - s_prev2;
+        s_prev2 = s_prev;
+        s_prev = s;
+    }
+    power = s_prev2*s_prev2+s_prev*s_prev-coeff*s_prev*s_prev2;
+    return power;
+}
+
+float goertzelFilter2(int8_t samples[], float freq, unsigned int N)
+{
+    float s_prev = 0.0;
+    float s_prev2 = 0.0;
+    float coeff,normalizedfreq,power,s;
+    unsigned int i;
+    normalizedfreq = freq / SAMPLEFREQUENCY;
+    coeff = 2*cos(2*M_PI*normalizedfreq);
+    for (i=0; i<N; i++) {
+        s = samples[i] + coeff * s_prev - s_prev2;
+        s_prev2 = s_prev;
+        s_prev = s;
+    }
+    power = s_prev2*s_prev2+s_prev*s_prev-coeff*s_prev*s_prev2;
+    return power;
+}
+
+void window_init(void)
+{
+	// TODO: setup dynamic allocation
+
+	for(unsigned int i=0;i<WINDOW_SIZE;i++)
+	{
+		window[i] = 0;
+	}
+}
+
+void window_add(int8_t sample)
+{
+	for(uint32_t i=0;i<(WINDOW_SIZE-1);i++)
+	{
+		window[(WINDOW_SIZE-1)-i] = window[(WINDOW_SIZE-2)-i];
+	}
+	window[0] = sample;
+}
+
+int8_t* window_get(void)
+{
+	return window;
+}
+
+unsigned int window_get_size(void)
+{
+	return sizeof(window);
+}
+

main/afsk_demodulator.h

@@ -0,0 +1,22 @@
+/*
+ * afsk_decoder.h
+ *
+ *  Created on: Sep 10, 2019
+ *      Author: curiousmuch
+ */
+
+#ifndef MAIN_AFSK_DEMODULATOR_H_
+#define MAIN_AFSK_DEMODULATOR_H_
+
+#define WINDOW_SIZE 5
+#define SAMPLEFREQUENCY 6000
+
+void window_init(void);
+void window_add(int8_t sample);
+int8_t* window_get(void);
+unsigned int window_get_size(void);
+float goertzelFilter2(int8_t samples[], float freq, unsigned int N);
+
+
+
+#endif /* MAIN_AFSK_DECODER_H_ */

main/bt_spp.c

@@ -18,6 +18,8 @@
 #include "esp_bt_device.h"
 #include "esp_spp_api.h"
 
+#include "bt_spp.h"
+
 // TODO: Remove and add call back functions to tie to main
 #include "kiss.c"
 
@@ -40,7 +42,7 @@ void esp_set_rx_cb(void (*rx_cb)(uint8_t *p, uint16_t p_len) )
 	receive_cb = rx_cb;
 }
 
-uint32_t handle = 0;
+uint32_t handle;
 
 // SPP Callback
 static void esp_spp_cb(esp_spp_cb_event_t event, esp_spp_cb_param_t *param)
@@ -68,14 +70,12 @@ static void esp_spp_cb(esp_spp_cb_event_t event, esp_spp_cb_param_t *param)
 	case ESP_SPP_CL_INIT_EVT:
 		// called when SPP client initiated a connection
 		ESP_LOGI(SPP_TAG, "ESP_SPP_CL_INIT_EVT handle=%d", param->open.handle);
-		handle = param->open.handle;
 		break;
 	case ESP_SPP_DATA_IND_EVT:
 		// called when SPP connection receives data
 		ESP_LOGI(SPP_TAG, "ESP_SPP_DATA_IND_EVT len=%d handle=%d", param->data_ind.len, param->data_ind.handle);
 		//esp_log_buffer_hex("", param->data_ind.data, param->data_ind.len);
 		kiss_receive(param->data_ind.data, param->data_ind.len);
-
 		// [DEBUGING] echo received data
 		// esp_spp_write(param->data_ind.handle, param->data_ind.len, param->data_ind.data);
 		break;
@@ -91,6 +91,7 @@ static void esp_spp_cb(esp_spp_cb_event_t event, esp_spp_cb_param_t *param)
 	case ESP_SPP_SRV_OPEN_EVT:
 		// called when SPP server connection opens
 		ESP_LOGI(SPP_TAG, "ESP_SPP_SRV_OPEN_EVT");
+    	handle = param->open.handle;
 		break;
 	default:
 		break;
@@ -151,7 +152,8 @@ void esp_bt_gap_cb(esp_bt_gap_cb_event_t event, esp_bt_gap_cb_param_t *param)
 
 void bt_spp_tx(uint8_t *buf, uint32_t len)
 {
-	esp_spp_write(handle, len, buf);
+//	if (handle != 0)
+		esp_spp_write(handle, len, buf);
 }
 
 void bt_spp_init(void)

main/bt_spp.h

@@ -0,0 +1,13 @@
+/*
+ * bt_spp.h
+ *
+ *  Created on: Sep 9, 2019
+ *      Author: curiousmuch
+ */
+
+#ifndef MAIN_BT_SPP_H_
+#define MAIN_BT_SPP_H_
+
+void bt_spp_tx(uint8_t *buf, uint32_t len);
+
+#endif /* MAIN_BT_SPP_H_ */

main/cc1200_protocol.h

@@ -174,17 +174,18 @@ static const cc1200_reg_settings_t APRS_TX_SETTINGS[]=
 
 static const cc1200_reg_settings_t APRS_RX2_SETTINGS[]=
 {
-  {CC120X_IOCFG2,            0x08},
-  {CC120X_IOCFG0,            0x09},
-  {CC120X_SYNC_CFG1,         0x3F},
-  {CC120X_DEVIATION_M,		 0x9D},
+  {CC120X_IOCFG2,            0x09},		// Serial RX
+  {CC120X_IOCFG3,			 29}, 	// Serial Clock
+  {CC120X_IOCFG0,            0x09},		// Serial RX/TX (Don't Touch)
+  {CC120X_SYNC_CFG1,         0x00},		// Disable Sync Word
+  {CC120X_DEVIATION_M,		 0x68},		// Deviation: 2.5kHz
   {CC120X_MODCFG_DEV_E,      0x00},
   {CC120X_DCFILT_CFG,        0x5D},
   {CC120X_PREAMBLE_CFG1,     0x00},
-  {CC120X_PREAMBLE_CFG0,     0x8A},
+  {CC120X_PREAMBLE_CFG0,     0x00},		// Preamble Disabled
   {CC120X_IQIC,              0xCB},
-  {CC120X_CHAN_BW,           0x9C},
-  {CC120X_MDMCFG1,           0x00},
+  {CC120X_CHAN_BW,           0xAA},//0x9C},
+  {CC120X_MDMCFG1,           0x06},
   {CC120X_MDMCFG0,           0x45},
   {CC120X_SYMBOL_RATE2,      0x3F},
   {CC120X_SYMBOL_RATE1,      0x75},
@@ -202,8 +203,8 @@ static const cc1200_reg_settings_t APRS_RX2_SETTINGS[]=
   {CC120X_PA_CFG1,           0x3F},
   {CC120X_PKT_LEN,           0xFF},
   {CC120X_IF_MIX_CFG,        0x1C},
-  {CC120X_FREQOFF_CFG,       0x22},
-  {CC120X_MDMCFG2,           0x0D},
+  {CC120X_FREQOFF_CFG,       0x22},		// Frequency Offset Configuration, Enabled: 0x22, Disabled: 0x02
+  {CC120X_MDMCFG2,           0x0D}, 	// CFM, Enabled: 0x0D, Disabled 0x0C
   {CC120X_FREQ2,             0x56},
   {CC120X_FREQ1,             0xA2},
   {CC120X_FREQ0,             0x4C},

main/kiss.c

@@ -15,28 +15,84 @@
 #include "cc1200.h"
 #include "fcs_calc.h"
 #include "kiss.h"
+#include "bt_spp.h"
 
-buffer_handle_t buffer_handle;
+kiss_buffer_t rx_buffer;
+kiss_buffer_t tx_buffer;
 tnc_settings_t tnc_settings;
 
+void static kiss_clear_tx_buffer(void)
+{
+	ESP_LOGD(TNC_TAG, "TNC buffer cleared.");
+	tx_buffer.index = 0;
+	tx_buffer.status = FRAME_INCOMPLETE;
+}
+
+void static kiss_append_tx_buffer(uint8_t chr)
+{
+	ESP_LOGD(TNC_TAG, "Appending %x to KISS TX buffer.", chr);
+	// ERROR - Packet Buffer Overflow
+	if (tx_buffer.index >= tx_buffer.max_len)
+	{
+		ESP_LOGE(TNC_TAG, "TNC TX buffer overflow");
+		kiss_clear_tx_buffer();
+	}
+	tx_buffer.buf[(tx_buffer.index)] = chr;
+	tx_buffer.index = tx_buffer.index + 1;
+}
+
+void kiss_transmit(uint8_t type, uint8_t *data, uint32_t len)
+{
+	uint32_t i;
+
+	// TODO: Cancel TX to BLE TX Buffer if overflow is detected
+
+	kiss_clear_tx_buffer();
+	kiss_append_tx_buffer(KISS_FEND);
+	kiss_append_tx_buffer(type);
+
+	// cycle
+	for (i=0; i<(len); i++)
+	{
+		if (data[i] == KISS_FEND)
+		{
+			kiss_append_tx_buffer(KISS_FESC);
+			kiss_append_tx_buffer(KISS_TFEND);
+		}
+		else if (data[i] == KISS_FESC)
+		{
+			kiss_append_tx_buffer(KISS_FESC);
+			kiss_append_tx_buffer(KISS_TFESC);
+		}
+		else
+		{
+			kiss_append_tx_buffer(data[i]);
+		}
+	}
+
+	kiss_append_tx_buffer(KISS_FEND);
+	bt_spp_tx(tx_buffer.buf, tx_buffer.index);
+
+	// send via BLE or feed to TX buffer
+}
 
 void kiss_clear_buffer(void)
 {
 	ESP_LOGD(TNC_TAG, "TNC buffer cleared.");
-	buffer_handle.index = 0;
+	rx_buffer.index = 0;
 }
 
 void kiss_append_buffer(uint8_t chr)
 {
 	ESP_LOGD(TNC_TAG, "Appending %x to TNC buffer.", chr);
 	// ERROR - Packet Buffer Overflow
-	if (buffer_handle.index >= buffer_handle.max_len)
+	if (rx_buffer.index >= rx_buffer.max_len)
 	{
 		ESP_LOGE(TNC_TAG, "TNC buffer overflow");
 		kiss_clear_buffer();
 	}
-	buffer_handle.buf[(buffer_handle.index)] = chr;
-	buffer_handle.index = buffer_handle.index + 1;
+	rx_buffer.buf[(rx_buffer.index)] = chr;
+	rx_buffer.index = rx_buffer.index + 1;
 }
 
 extern RingbufHandle_t radio_tx_buf;
@@ -46,51 +102,51 @@ extern RingbufHandle_t radio_tx_buf;
 void kiss_process_frame(void)
 {
     uint8_t data_byte, tnc_number;
-    data_byte = buffer_handle.buf[0];
+    data_byte = rx_buffer.buf[0];
     tnc_number = (data_byte >> 4) & 0x0F;	// TODO: Have KISS code respect TNC number
 
 	switch( data_byte ) {
 		case KISS_DATAFRAME: {
-			ESP_LOGI(TNC_TAG, "Received Data Frame - Length %d", (buffer_handle.index));
+			ESP_LOGI(TNC_TAG, "Received Data Frame - Length %d", (rx_buffer.index));
 			// unblock AX.25 code to process code
-			uint32_t fcs = fcs_calc(&buffer_handle.buf[1], buffer_handle.index-1);
-			buffer_handle.buf[buffer_handle.index] = fcs & 0xFF;
-			buffer_handle.buf[buffer_handle.index+1] = fcs>>8 & 0xFF;
-			xRingbufferSend(radio_tx_buf, &buffer_handle.buf[1], ((buffer_handle.index+1)*sizeof(uint8_t)), 10/portTICK_PERIOD_MS);
-			ESP_LOG_BUFFER_HEXDUMP(TNC_TAG, &buffer_handle.buf[1], buffer_handle.index+1, ESP_LOG_INFO);
-			//cc1200_radio_APRSTXPacket(buffer_handle.buf, (buffer_handle.index+2), tnc_settings.tx_delay, tnc_settings.tx_tail);
+			uint32_t fcs = fcs_calc(&rx_buffer.buf[1], rx_buffer.index-1);
+			rx_buffer.buf[rx_buffer.index] = fcs & 0xFF;
+			rx_buffer.buf[rx_buffer.index+1] = fcs>>8 & 0xFF;
+			xRingbufferSend(radio_tx_buf, &rx_buffer.buf[1], ((rx_buffer.index+1)*sizeof(uint8_t)), 10/portTICK_PERIOD_MS);
+			ESP_LOG_BUFFER_HEXDUMP(TNC_TAG, &rx_buffer.buf[1], rx_buffer.index+1, ESP_LOG_INFO);
+			//cc1200_radio_APRSTXPacket(rx_buffer.buf, (rx_buffer.index+2), tnc_settings.tx_delay, tnc_settings.tx_tail);
 			break;
 		}
 		case KISS_CMD_TXDELAY: {
 			ESP_LOGI(TNC_TAG, "Updated TX Delay");
-			tnc_settings.tx_delay = buffer_handle.buf[1];
+			tnc_settings.tx_delay = rx_buffer.buf[1];
 			break;
 		}
 		case KISS_CMD_P: {
 			ESP_LOGI(TNC_TAG, "Update Persistence");
-			tnc_settings.persistence = buffer_handle.buf[1];
+			tnc_settings.persistence = rx_buffer.buf[1];
 			break;
 		}
 		case KISS_CMD_SLOTTIME: {
 			ESP_LOGI(TNC_TAG, "Updated Slottime");
-			tnc_settings.slot_time = buffer_handle.buf[1];
+			tnc_settings.slot_time = rx_buffer.buf[1];
 			break;
 		}
 		case KISS_CMD_TXTAIL: {
 			ESP_LOGI(TNC_TAG, "Updated TX Tail");
-			tnc_settings.tx_tail= buffer_handle.buf[1];
+			tnc_settings.tx_tail= rx_buffer.buf[1];
 			break;
 		}
 		case KISS_CMD_FULLDUPLEX: {
 			ESP_LOGI(TNC_TAG, "Updated Full/Half Duplex Setting");
-			tnc_settings.full_duplex = buffer_handle.buf[1];
+			tnc_settings.full_duplex = rx_buffer.buf[1];
 			break;
 		}
 //		case KISS_CMD_SETHARDWARE: {
 //			break;
 //		}
 //		case KISS_CMD_RETURN: {
-//			tnc_settings.tx_delay = buffer_handle.buff_ptr[1];
+//			tnc_settings.tx_delay = rx_buffer.buff_ptr[1];
 //			break;
 //		}
 		default: {
@@ -112,14 +168,24 @@ void kiss_set_tnc_paramters(tnc_settings_t s)
 //		tnc_settings.tnc_number = s.tnc_number;
 }
 
+void kiss_set_tx_cb(tnc_cb_t tx_cb)
+{
+	tnc_settings.tx_callback = tx_cb;
+}
+
+void kiss_set_rx_cb(tnc_cb_t rx_cb)
+{
+	tnc_settings.rx_callback = rx_cb;
+}
 
 // TNC needs the ability to RX KISS frame, decode it, schedule it, and then send out via APRS or Arrow-Net
 // TNC needs the ability to switch to RX mode,
 
 void kiss_init(void)
 {
-	buffer_handle.max_len = FRAME_BUFFER_SIZE;			// TODO: make frame buffer dynamic
-//	tnc_settings.max_frame_size = FRAME_BUFFER_SIZE;
+	// TODO: make frame buffer dynamic
+	rx_buffer.max_len = FRAME_BUFFER_SIZE;
+	tx_buffer.max_len = FRAME_BUFFER_SIZE;
 
 	// TODO: Store / retrieve default TNC settings from flash?
 

main/kiss.h

@@ -40,12 +40,17 @@
 // SetHardware: Custom
 // Return: Exit Kiss
 
+typedef void (*tnc_cb_t)(uint8_t *, uint32_t);
+
  typedef struct {
 	uint8_t tx_delay;
 	uint8_t persistence;
 	uint8_t slot_time;
 	uint8_t tx_tail;
 	uint8_t full_duplex;
+	uint32_t spp_handle;
+	tnc_cb_t rx_callback;
+	tnc_cb_t tx_callback;
 } tnc_settings_t;
 
 
@@ -66,7 +71,7 @@ typedef struct {
 	uint32_t 	max_len;
 	uint32_t 	index;
   	FRAME_STATUS_t 	status;
-} buffer_handle_t;
+} kiss_buffer_t;
 
 typedef struct {
 	uint8_t *data;

main/main.c

@@ -28,11 +28,14 @@
 #include "ax25_pad.h"
 #include "fcs_calc.h"
 
+
 #include "math.h"
 
 #include "driver/timer.h"
 #include "aprs_decoder.h"
+#include "kiss.h"
 
+#include "afsk_demodulator.h"
 
 //#include "esp_dsp.h"
 
@@ -42,74 +45,17 @@ uint8_t APRS_TEST_PACKET[] = { 0x82, 0x98, 0x98, 0x40, 0x40, 0x40, 0xe0, 0x96, 0
 									 0x20, 0x69, 0x73, 0x20, 0x61, 0x20, 0x54, 0x65, 0x73, 0x74, 0x7b, 0x31, 0xad, 0xa1 };
 
 RingbufHandle_t radio_tx_buf;
+RingbufHandle_t radio_rx_buf;
+
 SemaphoreHandle_t xRadioRXISRSemaphore;
 SemaphoreHandle_t xRadioRXSemaphore;
 SemaphoreHandle_t xRadioTXSemaphore;
+SemaphoreHandle_t xRadioMutex;
 
 //RingbufHandle_t radio_rx_buf;
 
 extern int8_t EXTERNAL_DATA;
-
-#define WINDOW_SIZE 7
-#define SAMPLEFREQUENCY 6000
-
-int window[WINDOW_SIZE];
-int lpf_window[WINDOW_SIZE];
-
-double goertzelFilter(int samples[], double freq, int N)
-{
-    double s_prev = 0.0;
-    double s_prev2 = 0.0;
-    double coeff,normalizedfreq,power,s;
-    int i;
-    normalizedfreq = freq / SAMPLEFREQUENCY;
-    coeff = 2*cos(2*M_PI*normalizedfreq);
-    for (i=0; i<N; i++) {
-        s = samples[i] + coeff * s_prev - s_prev2;
-        s_prev2 = s_prev;
-        s_prev = s;
-    }
-    power = s_prev2*s_prev2+s_prev*s_prev-coeff*s_prev*s_prev2;
-    return power;
-}
-
-//double calculate_symbols_energy(int samples[], int N)
-//{
-//	double E_s1, E_s2;
-//	E_s1 = goertzelFilter(samples, 1200, N);
-//	E_s2 = goertzelFilter(samples, 2200, N);
-//	return (E_s1 - E_s2);
-//}
-
-void window_init(void)
-{
-	for(int i=0;i<WINDOW_SIZE;i++)
-	{
-		window[i] = 0;
-	}
-}
-
-void window_add(int sample)
-{
-	for(int i=0;i<(WINDOW_SIZE-1);i++)
-	{
-		window[(WINDOW_SIZE-1)-i] = window[(WINDOW_SIZE-2)-i];
-	}
-	window[0] = sample;
-}
-
-int* window_get(void)
-{
-	return window;
-}
-
-int window_get_size(void)
-{
-	return WINDOW_SIZE;
-}
-
-// APRS Decoder
-
+extern decoder_varibles_t v;
 
 // Timer Functions
 void IRAM_ATTR rx_timer_isr(void *para)
@@ -139,8 +85,7 @@ void IRAM_ATTR rx_timer_isr(void *para)
 
 }
 
-// The TX Task should have the highest
-void TX_Task(void *pvParameters)
+void Radio_Task(void *pvParameters)
 {
 	size_t p_size;
 
@@ -212,30 +157,89 @@ void TX_Task(void *pvParameters)
 	}
 }
 
+
 // Phase Locked Loop (PLL) Parameters
 #define BAUD_RATE (1200)
 #define SAMPLES_BIT (SAMPLEFREQUENCY / BAUD_RATE)
 #define D_PLL_INC (SAMPLES_BIT * 1)
 #define D_PLL_MAX (D_PLL_INC * SAMPLES_BIT *1)
-#define D_PLL_MARGIN (1)
+#define D_PLL_MARGIN (dpll_margin)
 
 uint8_t aprs_buf[256];
 
-void RX_Task(void *pvParameters)
+void dsps_biquad_gen_lpf_f32(float *coeffs, float f, float qFactor)
 {
-	// debugging variables
-	uint8_t toggle = 0;
-	uint8_t count = 0;
+    if (qFactor <= 0.0001) {
+        qFactor = 0.0001;
+    }
+    float Fs = 1;
+
+    float w0 = 2 * M_PI * f / Fs;
+    float c = cosf(w0);
+    float s = sinf(w0);
+    float alpha = s / (2 * qFactor);
+
+    float b0 = (1 - c) / 2;
+    float b1 = 1 - c;
+    float b2 = b0;
+    float a0 = 1 + alpha;
+    float a1 = -2 * c;
+    float a2 = 1 - alpha;
+
+    coeffs[0] = b0 / a0;
+    coeffs[1] = b1 / a0;
+    coeffs[2] = b2 / a0;
+    coeffs[3] = a1 / a0;
+    coeffs[4] = a2 / a0;
+    return;
+}
 
+void dsps_biquad_f32_ansi(const float *input, float *output, int len, float *coef, float *w)
+{
+    for (int i = 0 ; i < len ; i++) {
+        float d0 = input[i] - coef[3] * w[0] - coef[4] * w[1];
+        output[i] = coef[0] * d0 +  coef[1] * w[0] + coef[2] * w[1];
+        w[1] = w[0];
+        w[0] = d0;
+    }
+    return;
+}
+
+float lpf_coef[5];
+float lpf_delay[2];
+
+void lpf_init(void)
+{
+	// generate filter coef
+	dsps_biquad_gen_lpf_f32(lpf_coef, 0.1999, 1);
+
+	// setup delay line and lpf_input window
+	int i;
+	for(i=0;i<sizeof(lpf_delay)/sizeof(float);i++)
+	{
+		lpf_delay[i] = 0;
+	}
+}
+
+float lpf_baseband(float input)
+{
+	float output;
+	dsps_biquad_f32_ansi(&input, &output, 1, lpf_coef, lpf_delay);
+	return output;
+}
+
+
+void RX_DSP_Task(void *pvParameters)
+{
 	// algorithm variables
-	double E_s1=0, E_s2=0;
+	volatile float E_s1=0, E_s2=0, lpf_output;
 	uint8_t raw_bit=0, previous_raw_bit=0;
-	int32_t d_pll=0, dpll_error=0, err_div=2;
-	int lpf_input = 0;
-	int lpf_output = 0;
+	volatile int32_t d_pll=0, dpll_error=0, err_div=0, dpll_margin = 1;
+	uint32_t success = 0;
 
 	aprs_decoder_init();
 	frame_buffer_init(aprs_buf, 256);
+	lpf_init();
 
 	// Sampling Semaphore
 	xRadioRXISRSemaphore = xSemaphoreCreateBinary();
@@ -246,53 +250,64 @@ void RX_Task(void *pvParameters)
 		{
 
 			//enable_debug_IO(DEBUG_0);
-			EXTERNAL_DATA = (int)cc1200_radio_read_CFM();
+			if (xSemaphoreTake(xRadioMutex, 0) == pdTRUE)
+			{
+				//disable_debug_IO(DEBUG_0);
+				//enable_debug_IO(DEBUG_0);
+				EXTERNAL_DATA = (int)cc1200_radio_read_CFM();
+				xSemaphoreGive(xRadioMutex);
+				//disable_debug_IO(DEBUG_0);
+			}
+			else
+			{
+				ESP_LOGE("Radio", "Sampling Failure");
+			}
 			//disable_debug_IO(DEBUG_0);
 			//enable_debug_IO(DEBUG_0);
-			//lpf_output += (50000 * (((int)EXTERNAL_DATA) - lpf_output)) / 100000;
-			window_add((int) EXTERNAL_DATA);
-			E_s1 = goertzelFilter(window, 1200, WINDOW_SIZE);
-			E_s2 = goertzelFilter(window, 2200, WINDOW_SIZE);
 
-			if (E_s1 > E_s2)
+			window_add(EXTERNAL_DATA);
+			int8_t *window = window_get();
+			E_s1 = goertzelFilter2(window, 1200, WINDOW_SIZE);
+			E_s2 = goertzelFilter2(window, 2200, WINDOW_SIZE);
+			lpf_output = lpf_baseband((E_s1 - E_s2));
+
+			if (lpf_output > 0)
 			{
 				raw_bit = 1;
 				enable_debug_IO(DEBUG_2);
-				enable_debug_IO(DEBUG_0);
 
 			}
 			else
 			{
 				raw_bit = 0;
 				disable_debug_IO(DEBUG_2);
-				disable_debug_IO(DEBUG_0);
 			}
 
 			//disable_debug_IO(DEBUG_0);
 			//enable_debug_IO(DEBUG_0);
+
 			// DPLL for sampling
 			if (raw_bit != previous_raw_bit)
 			{
 				dpll_error = d_pll - (D_PLL_MAX / 2);
 				if (dpll_error >  (D_PLL_MARGIN))
 				{
-					d_pll -= get_rx_status() ? D_PLL_MARGIN:
-							(dpll_error + err_div / 2) / err_div;
+					d_pll -= get_rx_status()  ? D_PLL_MARGIN:
+							(err_div + dpll_error);//(dpll_error + err_div / 2) / err_div;
 				}
 				else if (dpll_error < (-D_PLL_MARGIN))
 				{
-					d_pll += get_rx_status() ? D_PLL_MARGIN:
-							(dpll_error + err_div / 2) / err_div;
+					d_pll += get_rx_status()  ? D_PLL_MARGIN:
+							-(err_div + dpll_error); //(dpll_error + err_div / 2) / err_div;
 				}
 			}
 
 			d_pll += D_PLL_INC;
 
-			disable_debug_IO(DEBUG_3);
 			if (d_pll >= D_PLL_MAX)
 			{
 				// set clock debug I/O high
-				enable_debug_IO(DEBUG_3);
+				enable_debug_IO(DEBUG_0);
 
 				// feed bit to aprs decoder algorithm
 				switch(aprs_decoder_feed_bit(raw_bit))
@@ -300,9 +315,10 @@ void RX_Task(void *pvParameters)
 					case NORMAL:
 						break;
 					case FRAME_DECODED:
-						ESP_LOGI("APRS RX", "AX.25 Frame Received");
+						ESP_LOGI("APRS RX", "AX.25 Frame Received [%d]", success++);
 						// send via KISS TNC to over BLE SPP
 						//ESP_LOG_BUFFER_HEXDUMP("APRS RX", aprs_buf, 100, ESP_LOG_INFO);
+						kiss_transmit(KISS_DATAFRAME, v.frame_buffer, v.frame_len);
 						break;
 					case ERROR_FCS_MISMATCH:
 						ESP_LOGV("APRS RX", "AX.25 FCS Error\n");
@@ -317,22 +333,25 @@ void RX_Task(void *pvParameters)
 			else
 			{
 				// set clock debug I/O low
+				disable_debug_IO(DEBUG_0);
 			}
 			previous_raw_bit = raw_bit;
-			//disable_debug_IO(DEBUG_0);
 		}
 	}
 }
 
+
 void radio_init()
 {
 	// Setup Task Communications
 	radio_tx_buf = xRingbufferCreate(1028, RINGBUF_TYPE_NOSPLIT);
+	radio_rx_buf = xRingbufferCreate(1028, RINGBUF_TYPE_NOSPLIT);
 	xRadioRXSemaphore = xSemaphoreCreateBinary();
 	xRadioTXSemaphore = xSemaphoreCreateBinary();
+	xRadioMutex = xSemaphoreCreateMutex();
 
-	xTaskCreatePinnedToCore(TX_Task, "TX Task", 1024*4, 0, 2, NULL, 1);
-	xTaskCreatePinnedToCore(RX_Task, "RX Task", 1024*4, 0, 1, NULL, 1);
+	xTaskCreatePinnedToCore(Radio_Task, "Radio Controller", 1024*4, 0, 2, NULL, 1);
+	xTaskCreatePinnedToCore(RX_DSP_Task, "Radio DSP", 1024*4, 0, 1, NULL, 1);
 
 }
 
@@ -352,9 +371,12 @@ void IRAM_ATTR app_main()
 	// Radio Task Initialize
 	radio_init();
 
-	// Setup Kiss Decoder and Encoder
+	// AFSK Decoder and Encoder
+
+
+	// Kiss Decoder and Encoder
 	kiss_init();
 
-	// Initalize BLE
+	// BLE and SPP
 	bt_spp_init();
 }