RX tone detection is working, but it's causing a Queue Assert failure which results in the entire bloody thing crashing? I believe the RX TX functionality needs to be split into two different tasks. Progress :). Another sad note. The frequency calculation is eating a tone of time. I needed to reduce the sample rate to 6kHz to keep up. I haven't tested with noise, but from the sig gen it seems reliableeven at -110dBm. I don't know what the voice FM equilvents are.

main/cc1200.c

@@ -46,7 +46,7 @@ spi_device_interface_config_t interface_config =
 	.dummy_bits = 0,
 	.mode = 0,
 	.spics_io_num = CC1120_CS,
-	.clock_speed_hz = (APB_CLK_FREQ/8),
+	.clock_speed_hz = (APB_CLK_FREQ/16),
 	.flags = 0,
 	.queue_size = 20
 
@@ -180,6 +180,20 @@ void IRAM_ATTR cc1200_spi_read_byte(uint16_t addr, uint8_t* data)
 	ESP_ERROR_CHECK(ret);
 }
 
+uint8_t IRAM_ATTR cc1200_radio_read_RSSI(void)
+{
+	uint8_t data = 0;
+	cc1200_spi_read_byte(CC120X_RSSI1, &data);
+	return data;
+}
+
+uint8_t IRAM_ATTR cc1200_radio_read_CFM(void)
+{
+	uint8_t data = 0;
+	cc1200_spi_read_byte(CC120X_CFM_RX_DATA_OUT, &data);
+	return data;
+}
+
 void cc1200_spi_read_bytes(uint16_t addr, uint8_t* data, uint8_t len)
 {
 	esp_err_t ret;
@@ -396,17 +410,20 @@ void IRAM_ATTR cc1200_radio_APRSTXPacket(uint8_t  *f, uint16_t f_len, uint8_t tx
 	vTaskSuspendAll();
 
 	// setup data rate for CFM TX
-	cc1200_radio_write(APRS_TX_RATE, sizeof(APRS_TX_RATE)/sizeof(cc1200_reg_settings_t));
+	cc1200_radio_write(APRS_TX_SETTINGS, sizeof(APRS_TX_SETTINGS)/sizeof(cc1200_reg_settings_t));
+	cc1200_radio_frequency(144390000-6000);
+
 
 	// start CW transmission
 	cc1200_spi_write_byte(CC120X_FIFO, 0x12);
-	cc1200_spi_strobe(CC120X_STX);
+	while(cc1200_spi_strobe(CC120X_STX) != CC120X_STATE_TX);
 
 	// enable interrupt pin for CC1120 for timing packets
 	gpio_install_isr_service(ESP_INTR_FLAG_IRAM);
-
 	gpio_isr_handler_add(CC1120_GPIO3, cc1200_aprs_tx_isr, NULL);
 	gpio_set_intr_type(CC1120_GPIO3, GPIO_INTR_POSEDGE);
+	gpio_intr_enable(CC1120_GPIO3);
+
 
 
 
@@ -414,98 +431,70 @@ void IRAM_ATTR cc1200_radio_APRSTXPacket(uint8_t  *f, uint16_t f_len, uint8_t tx
 	uint16_t p_len = tx_delay * 12;
 	uint16_t t_len = tx_tail * 12 + 1;
 
-		// Start CW transmission
-		cc1200_spi_write_byte(CC120X_FIFO, 0x12);
-		cc1200_spi_strobe(CC120X_STX);
+    // Start CW transmission
+	cc1200_spi_write_byte(CC120X_FIFO, 0x12);
+	cc1200_spi_strobe(CC120X_STX);
 
-		sample_count = 0;
-		new_sample = 0;
+	sample_count = 0;
+	new_sample = 0;
 
-		// Send HDLC Flag for TX Delay Time
-		for (i = 0; i<p_len; i++)
+	// Send HDLC Flag for TX Delay Time
+	for (i = 0; i<p_len; i++)
+	{
+		esp_task_wdt_reset();
+		aprs_flags.byte = 0x7E;
+		for(j=0; j<8; j++)
 		{
-			esp_task_wdt_reset();
-			aprs_flags.byte = 0x7E;
-			for(j=0; j<8; j++)
-			{
-				aprs_flags.cur_bit = aprs_flags.byte & 0x01;
+			aprs_flags.cur_bit = aprs_flags.byte & 0x01;
 
-				// NRZ-I Encoding
-				if (aprs_flags.cur_bit)
-				{
-					// do nothing
-					aprs_flags.one_count++;
+			// NRZ-I Encoding
+			if (aprs_flags.cur_bit)
+			{
+				// do nothing
+				aprs_flags.one_count++;
 
-				}
-				else
-				{
-					aprs_flags.tone = aprs_flags.tone ^ 1; // switch tone
-					aprs_flags.one_count = 0;
-				}
+			}
+			else
+			{
+				aprs_flags.tone = aprs_flags.tone ^ 1; // switch tone
+				aprs_flags.one_count = 0;
+			}
 
-				aprs_flags.byte = (aprs_flags.byte >> 1);
+			aprs_flags.byte = (aprs_flags.byte >> 1);
 
-				while(sample_count < 11)	// wait for symbol to be sent
+			while(sample_count < 11)	// wait for symbol to be sent
+			{
+				if ( new_sample )
 				{
-					if ( new_sample )
-					{
-						LUT_lookup();
-						new_sample = 0;
-					}
+					LUT_lookup();
+					new_sample = 0;
 				}
-				sample_count = 0;
-				//printf("Symbol: %x\n", aprs_flags.cur_bit);
-
 			}
+			sample_count = 0;
+			//printf("Symbol: %x\n", aprs_flags.cur_bit);
+
 		}
-		aprs_flags.one_count = 0;
+	}
+	aprs_flags.one_count = 0;
 
-		// Send Packet / Frame
-		//for (i=0;i<aprs_flags.packet_len;i++)
-		for (i=0;i<f_len;i++)
+	// Send Packet / Frame
+	//for (i=0;i<aprs_flags.packet_len;i++)
+	for (i=0;i<f_len;i++)
+	{
+		esp_task_wdt_reset();
+		aprs_flags.byte = f[i];
+		for(j=0; j<8; j++)
 		{
-			esp_task_wdt_reset();
-			aprs_flags.byte = f[i];
-			for(j=0; j<8; j++)
-			{
-				aprs_flags.cur_bit = aprs_flags.byte & 0x01;	// bool of first bit
-
-				// Zero Stuffing
-				if (aprs_flags.one_count == 5)
-				{
-					aprs_flags.tone = aprs_flags.tone ^ 1;
-					aprs_flags.one_count = 0;
-
-					// wait for symbol to be sent
-					while(sample_count < 11)
-					{
-						if ( new_sample )
-						{
-							LUT_lookup();
-							new_sample = 0;
-						}
-					}
-					toggle2 = toggle2 ^ 1;
-					//gpio_set_level(DEBUG_0, toggle2);
-					sample_count = 0;
-				}
-
-				// NRZ-I Encoding
-				if (aprs_flags.cur_bit)
-				{
-					// do nothing
-					aprs_flags.one_count++;
-
-				}
-				else
-				{
-					aprs_flags.tone = aprs_flags.tone ^ 1; // switch tone
-					aprs_flags.one_count = 0;
-				}
+			aprs_flags.cur_bit = aprs_flags.byte & 0x01;	// bool of first bit
 
-				aprs_flags.byte = (aprs_flags.byte >> 1);
+			// Zero Stuffing
+			if (aprs_flags.one_count == 5)
+			{
+				aprs_flags.tone = aprs_flags.tone ^ 1;
+				aprs_flags.one_count = 0;
 
-				while(sample_count < 11)	// wait for symbol to be sent
+				// wait for symbol to be sent
+				while(sample_count < 11)
 				{
 					if ( new_sample )
 					{
@@ -516,73 +505,105 @@ void IRAM_ATTR cc1200_radio_APRSTXPacket(uint8_t  *f, uint16_t f_len, uint8_t tx
 				toggle2 = toggle2 ^ 1;
 				//gpio_set_level(DEBUG_0, toggle2);
 				sample_count = 0;
-				//printf("Symbol: %x\n", aprs_flags.cur_bit);
 			}
-		}
-		aprs_flags.one_count = 0;
 
-		// Send HDLC Flag for TX Tail
-		for (i = 0; i<t_len; i++)
-		{
-			esp_task_wdt_reset();
-			aprs_flags.byte = 0x7E;
-			for(j=0; j<8;j++)
+			// NRZ-I Encoding
+			if (aprs_flags.cur_bit)
 			{
-				aprs_flags.cur_bit = aprs_flags.byte & 0x01;
+				// do nothing
+				aprs_flags.one_count++;
 
-				// NRZ-I Encoding
-				if (aprs_flags.cur_bit)
-				{
-					// do nothing
-					aprs_flags.one_count++;
+			}
+			else
+			{
+				aprs_flags.tone = aprs_flags.tone ^ 1; // switch tone
+				aprs_flags.one_count = 0;
+			}
 
-				}
-				else
+			aprs_flags.byte = (aprs_flags.byte >> 1);
+
+			while(sample_count < 11)	// wait for symbol to be sent
+			{
+				if ( new_sample )
 				{
-					aprs_flags.tone = aprs_flags.tone ^ 1; // switch tone
-					aprs_flags.one_count = 0;
+					LUT_lookup();
+					new_sample = 0;
 				}
+			}
+			toggle2 = toggle2 ^ 1;
+			//gpio_set_level(DEBUG_0, toggle2);
+			sample_count = 0;
+			//printf("Symbol: %x\n", aprs_flags.cur_bit);
+		}
+	}
+	aprs_flags.one_count = 0;
+
+	// Send HDLC Flag for TX Tail
+	for (i = 0; i<t_len; i++)
+	{
+		esp_task_wdt_reset();
+		aprs_flags.byte = 0x7E;
+		for(j=0; j<8;j++)
+		{
+			aprs_flags.cur_bit = aprs_flags.byte & 0x01;
 
-				aprs_flags.byte = (aprs_flags.byte >> 1);
+			// NRZ-I Encoding
+			if (aprs_flags.cur_bit)
+			{
+				// do nothing
+				aprs_flags.one_count++;
 
-				while(sample_count < 11)	// wait for symbol to be sent
+			}
+			else
+			{
+				aprs_flags.tone = aprs_flags.tone ^ 1; // switch tone
+				aprs_flags.one_count = 0;
+			}
+
+			aprs_flags.byte = (aprs_flags.byte >> 1);
+
+			while(sample_count < 11)	// wait for symbol to be sent
+			{
+				if ( new_sample )
 				{
-					if ( new_sample )
-					{
-						LUT_lookup();
-						new_sample = 0;
-					}
+					LUT_lookup();
+					new_sample = 0;
 				}
-				sample_count = 0;
-
 			}
+			sample_count = 0;
+
 		}
-		cc1200_spi_strobe(CC120X_SIDLE);
-		//cc1200_spi_strobe(CC120X_SFTX);
-		xTaskResumeAll();
-		spi_device_release_bus(spi);
-		gpio_uninstall_isr_service();
+	}
+
+	gpio_intr_disable(CC1120_GPIO3);
+	gpio_set_intr_type(CC1120_GPIO3, GPIO_INTR_DISABLE);
+	while(cc1200_spi_strobe(CC120X_SIDLE)!= CC120X_STATE_IDLE);
+	xTaskResumeAll();
+	spi_device_release_bus(spi);
+	gpio_uninstall_isr_service();
 }
 
 extern SemaphoreHandle_t xRadioRXSemaphore;
+int8_t EXTERNAL_DATA;
 
 static void IRAM_ATTR cc1200_aprs_rx_isr(void* arg)
 {
 
 	//uint8_t data = 0;
     //cc1200_spi_read_byte(CC120X_CFM_RX_DATA_OUT, &data);
-	static BaseType_t xHigherPriorityTaskWoken;
+	static BaseType_t xHigherPriorityTaskWoken = pdFALSE;
 
 
-	gpio_set_intr_type(CC1120_GPIO2, GPIO_INTR_DISABLE);
+	//gpio_set_intr_type(CC1120_GPIO2, GPIO_INTR_DISABLE);
 
+	cc1200_spi_read_byte(CC120X_CFM_RX_DATA_OUT, (uint8_t *)&EXTERNAL_DATA);
 	xSemaphoreGiveFromISR(xRadioRXSemaphore, &xHigherPriorityTaskWoken);
 	if (xHigherPriorityTaskWoken == pdTRUE)
 	{
 		portYIELD_FROM_ISR( );
 	}
 
-	//cc1200_spi_read_byte(CC120X_RSSI1, &data);
+
 
 	//toggle = toggle ^ 1;
 	//	gpio_set_level(DEBUG_0, toggle);
@@ -598,24 +619,28 @@ void IRAM_ATTR cc1200_radio_start_APRSRX(void)
 	// acquire SPI bus for fastest possible SPI transactions
 	spi_device_acquire_bus(spi, portMAX_DELAY);
 
-	//vTaskSuspendAll();
 
 	// start RX transmission
-	cc1200_radio_write(APRS_RX_RATE, sizeof(APRS_RX_RATE)/sizeof(cc1200_reg_settings_t));
-	while(cc1200_spi_strobe(CC120X_SRX) != CC120X_STATE_RX);
+	cc1200_radio_write(APRS_RX_SETTINGS, sizeof(APRS_RX_SETTINGS)/sizeof(cc1200_reg_settings_t));
+	cc1200_radio_frequency(144390000-6000);
 
-
-	// enable interrupt pin for CC1120 for timing packets
+	// enable ISR for CC1120 for timing packets
 	gpio_install_isr_service(ESP_INTR_FLAG_IRAM);
-
 	gpio_isr_handler_add(CC1120_GPIO2, cc1200_aprs_rx_isr, NULL);
 	gpio_set_intr_type(CC1120_GPIO2, GPIO_INTR_POSEDGE);
+	gpio_intr_enable(CC1120_GPIO2);
+
+	while(cc1200_spi_strobe(CC120X_SRX) != CC120X_STATE_RX);
 }
 
 void cc1200_radio_stop_APRSRX(void)
 {
+	// disable interrupt
+	gpio_intr_disable(CC1120_GPIO2);
+	gpio_set_intr_type(CC1120_GPIO2, GPIO_INTR_DISABLE);
 	gpio_uninstall_isr_service();
-	cc1200_spi_strobe(CC120X_SIDLE);
+
+	while(cc1200_spi_strobe(CC120X_SIDLE) != CC120X_STATE_IDLE);
 	spi_device_release_bus(spi);
 }
 
@@ -627,9 +652,9 @@ void cc1200_radio_init(const cc1200_reg_settings_t* rf_settings, uint8_t len)
 
 	cc1200_radio_reset();	gpio_set_level(CC1120_RESET, 1);
 
-	uint8_t data;
-	cc1200_spi_read_byte(CC120X_PARTNUMBER, &data);
-	printf("%x", data);
+	//uint8_t data;
+	//cc1200_spi_read_byte(CC120X_PARTNUMBER, &data);
+	//printf("%x", data);
 
 
 	uint8_t i;
@@ -638,6 +663,8 @@ void cc1200_radio_init(const cc1200_reg_settings_t* rf_settings, uint8_t len)
 	{
 		cc1200_spi_write_byte(rf_settings[i].addr, rf_settings[i].data);
 	}
+	while(cc1200_spi_strobe(CC120X_SIDLE) != CC120X_STATE_IDLE);
+
 }
 
 

main/cc1200.h

@@ -292,6 +292,9 @@ esp_err_t cc1200_radio_sleep(void);
 void cc1200_radio_APRSTXPacket(uint8_t  *f, uint16_t f_len, uint8_t tx_delay, uint8_t tx_tail);
 void cc1200_radio_start_APRSRX(void);
 void cc1200_radio_stop_APRSRX(void);
+uint8_t cc1200_radio_read_RSSI(void);
+uint8_t cc1200_radio_read_CFM(void);
+
 
 #endif// CC120X_SPI_H
 

main/cc1200_protocol.h

@@ -48,21 +48,72 @@ static const cc1200_reg_settings_t APRS_RX_RATE[]=
 		{CC120X_SYMBOL_RATE0, 0x7B}
 };
 
-static const cc1200_reg_settings_t APRS_SETTINGS[]=
+static const cc1200_reg_settings_t APRS_RX_SETTINGS[]=
 {
   {CC120X_IOCFG3,            0x1e},	// TX Clock
   {CC120X_IOCFG2,			 0x1d},	// RX Clock
   {CC120X_IOCFG0,            0x09},
-  {CC120X_SYNC_CFG1,         0xAB},	// Disable Sync Word [0x00]
-  //{CC120X_SYNC_CFG0,		 0x03}, // Disable RX_CONFIG_LIMITATION 0x13
+  {CC120X_SYNC_CFG1,         0x00},	// Disable Sync Word [0x00]
+  {CC120X_SYNC_CFG0,		 0x13}, // Disable RX_CONFIG_LIMITATION 0x13
   {CC120X_DEVIATION_M,       0x9D},
   {CC120X_MODCFG_DEV_E,      0x00},
   {CC120X_DCFILT_CFG,        0x5D},
   {CC120X_PREAMBLE_CFG1,     0x00},
-  {CC120X_PREAMBLE_CFG0,     0x8A},	// Disable Preamble [0x00]
+  {CC120X_PREAMBLE_CFG0,     0x00},	// Disable Preamble [0x00]
+  {CC120X_IQIC,              0xCB},
+  {CC120X_CHAN_BW,           0x9C},
+  {CC120X_MDMCFG1, 			 0x00},	// Random guess
+  {CC120X_MDMCFG0,           0x05},
+  {CC120X_SYMBOL_RATE2,      0x63}, //0x75},
+  {CC120X_SYMBOL_RATE1,      0xA9}, //0xA0},
+  {CC120X_SYMBOL_RATE0,      0x2A}, //0x7B},
+  {CC120X_AGC_REF,           0x30},
+  {CC120X_AGC_CS_THR,        0xEC},
+  {CC120X_AGC_CFG1,          0x51},
+  {CC120X_AGC_CFG0,          0x87},
+  {CC120X_FIFO_CFG,          0x00},
+  {CC120X_FS_CFG,            0x1B},
+  {CC120X_PKT_CFG2,          0x01},
+  {CC120X_PKT_CFG1,          0x00},
+  {CC120X_PKT_CFG0,          0x20},
+  {CC120X_PA_CFG1,           0x3F},
+  {CC120X_PKT_LEN,           0xFF},
+  {CC120X_IF_MIX_CFG,        0x1C},
+  {CC120X_FREQOFF_CFG,       0x22},
+  {CC120X_MDMCFG2,           0x0B},		// need to set CFM_DATA_EN bit for CFM mode and reduce upsampler rate
+  {CC120X_FREQ2,             0x56},
+  {CC120X_FREQ1,             0x81},
+  {CC120X_FREQ0,             0x47},
+  {CC120X_IF_ADC1,           0xEE},
+  {CC120X_IF_ADC0,           0x10},
+  {CC120X_FS_DIG1,           0x07},
+  {CC120X_FS_DIG0,           0xAF},
+  {CC120X_FS_CAL1,           0x40},
+  {CC120X_FS_CAL0,           0x0E},
+  {CC120X_FS_DIVTWO,         0x03},
+  {CC120X_FS_DSM0,           0x33},
+  {CC120X_FS_DVC0,           0x17},
+  {CC120X_FS_PFD,            0x00},
+  {CC120X_FS_PRE,            0x6E},
+  {CC120X_FS_REG_DIV_CML,    0x1C},
+  {CC120X_FS_SPARE,          0xAC},
+  {CC120X_FS_VCO0,           0xB5},
+  {CC120X_XOSC5,             0x0E},
+  {CC120X_XOSC1,             0x03},
+};
+
+static const cc1200_reg_settings_t APRS_TX_SETTINGS[]=
+{
+  {CC120X_IOCFG3,            0x1e},	// 0x1e for TX 0x1D for RX
+  {CC120X_IOCFG0,            0x1d},
+  {CC120X_SYNC_CFG1,         0xAB},
+  {CC120X_DEVIATION_M,       0x9D},
+  {CC120X_MODCFG_DEV_E,      0x00},
+  {CC120X_DCFILT_CFG,        0x5D},
+  {CC120X_PREAMBLE_CFG1,     0x00},
+  {CC120X_PREAMBLE_CFG0,     0x8A},
   {CC120X_IQIC,              0xCB},
   {CC120X_CHAN_BW,           0x9C},
-  //{CC120X_MDMCFG1, 			 0x00},	// Random guess
   {CC120X_MDMCFG0,           0x05},
   {CC120X_SYMBOL_RATE2,      0x35},
   {CC120X_SYMBOL_RATE1,      0xA0},
@@ -80,7 +131,7 @@ static const cc1200_reg_settings_t APRS_SETTINGS[]=
   {CC120X_PKT_LEN,           0x3F},
   {CC120X_IF_MIX_CFG,        0x1C},
   {CC120X_FREQOFF_CFG,       0x22},
-  {CC120X_MDMCFG2,           0x0D},		// need to set CFM_DATA_EN bit for CFM mode and reduce upsampler rate
+  {CC120X_MDMCFG2,           0x0D},		// need to set CFM_DATA_EN bit for CFM mode
   {CC120X_FREQ2,             0x56},
   {CC120X_FREQ1,             0x81},
   {CC120X_FREQ0,             0x47},
@@ -102,3 +153,4 @@ static const cc1200_reg_settings_t APRS_SETTINGS[]=
   {CC120X_XOSC1,             0x03},
 };
 
+

main/main.c

@@ -28,6 +28,8 @@
 #include "ax25_pad.h"
 #include "fcs_calc.h"
 
+#include "math.h"
+
 uint8_t APRS_TEST_PACKET[] = { 0x82, 0x98, 0x98, 0x40, 0x40, 0x40, 0xe0, 0x96, 0x84, 0x66, 0xaa, 0x96, 0xac, 0xe0, 0xae, 0x92,
 									 0x88, 0x8a, 0x62, 0x40, 0x62, 0xae, 0x92, 0x88, 0x8a, 0x64, 0x40, 0x65, 0x03, 0xf0, 0x3a, 0x4b,
 									 0x42, 0x33, 0x55, 0x4b, 0x56, 0x2d, 0x32, 0x20, 0x3a, 0x48, 0x69, 0x21, 0x20, 0x54, 0x68, 0x69, 0x73,
@@ -37,33 +39,116 @@ RingbufHandle_t radio_tx_buf;
 SemaphoreHandle_t xRadioRXSemaphore;
 //RingbufHandle_t radio_rx_buf;
 
+extern int8_t EXTERNAL_DATA;
+
+#define WINDOW_SIZE 11
+#define SAMPLEFREQUENCY 6000
+
+int 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;
+}
+
+uint8_t * window_get(void)
+{
+	return window;
+}
+
+int window_get_size(void)
+{
+	return WINDOW_SIZE;
+}
+
 void Radio_Task(void *pvParameters)
 {
 	size_t p_size;
 
-	// create sempahore
-	xRadioRXSemaphore = xSemaphoreCreateBinary();
-
 	// Setup Radio
-	cc1200_radio_init(APRS_SETTINGS, sizeof(APRS_SETTINGS)/sizeof(cc1200_reg_settings_t));
+	xRadioRXSemaphore = xSemaphoreCreateBinary();
+	cc1200_radio_init(APRS_RX_SETTINGS, sizeof(APRS_RX_SETTINGS)/sizeof(cc1200_reg_settings_t));
 	cc1200_radio_frequency(144390000-6000);
 
 	cc1200_radio_start_APRSRX();
 
 	uint8_t toggle=0;
+	uint8_t count = 0;
+
+	double E_s1, E_s2;
 
 	while(1)
 	{
-		// setup LEDs for RX mode
-		enable_green_led();
-		disable_red_led();
+//		// setup LEDs for RX mode
+//		enable_green_led();
+//		disable_red_led();
 
 		//cc1200_radio_start_APRSRX();
 		if (xSemaphoreTake(xRadioRXSemaphore, 0) == pdTRUE)
 		{
+			//gpio_set_level(DEBUG_0, 1);
+			gpio_intr_disable(CC1120_GPIO2);
 			toggle = toggle ^ 1;
 			gpio_set_level(DEBUG_0, toggle);
-			gpio_set_intr_type(CC1120_GPIO2, GPIO_INTR_POSEDGE);
+
+
+			//data = cc1200_radio_read_CFM();
+			window_add((int) EXTERNAL_DATA);
+
+			E_s1 = goertzelFilter(window, 1200, WINDOW_SIZE);
+			E_s2 = goertzelFilter(window, 2200, WINDOW_SIZE);
+			//printf("%f\n", (E_s1 - E_s2));
+
+			if (E_s1 > E_s2 )
+			{
+
+				enable_green_led();
+				disable_red_led();
+			}
+			else
+			{
+				enable_red_led();
+				disable_green_led();
+			}
+
+			//gpio_set_level(DEBUG_0, 0);
+
 		}
 
 		//esp_task_wdt_reset();
@@ -76,9 +161,9 @@ void Radio_Task(void *pvParameters)
 			// disable RX mode
 			cc1200_radio_stop_APRSRX();
 
-			// setup LEDs for TX mode
-			enable_red_led();
-			disable_green_led();
+//			// setup LEDs for TX mode
+//			enable_red_led();
+//			disable_green_led();
 
 
 			cc1200_radio_APRSTXPacket(p, p_size, 2, 0);
@@ -92,15 +177,19 @@ void Radio_Task(void *pvParameters)
 
 //void UART_Task(void *pvParameters)
 //{
-//
+//	printf("RSSI: %x", data);
 //}
-//
 
 
-void radio_task_init()
+
+void radio_init()
 {
+	// Setup Task Communications
 	radio_tx_buf = xRingbufferCreate(1028, RINGBUF_TYPE_NOSPLIT);
+
+	// Create Tasks
 	xTaskCreatePinnedToCore(Radio_Task, "Radio_Task", 1024*4, 0, 1, NULL, 1);
+//	xTaskCreatePinnedToCore(UART_Task, "UART Task", 1024*4, 0, 5, NULL, 0);
 }
 
 void IRAM_ATTR app_main()
@@ -117,7 +206,7 @@ void IRAM_ATTR app_main()
 	board_init();
 
 	// Radio Task Initialize
-	radio_task_init();
+	radio_init();
 
 	// Setup Kiss Decoder and Encoder
 	kiss_init();