diff --git a/.vscode/settings.json b/.vscode/settings.json
index 098ea5a..bdd00a8 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -16,6 +16,10 @@
         "pwr_level.h": "c",
         "fsl_ctimer.h": "c",
         "soft_timer.h": "c",
-        "eeprom.h": "c"
+        "eeprom.h": "c",
+        "dds.h": "c",
+        "frq.h": "c",
+        "driver.h": "c",
+        "pwm.h": "c"
     }
 }
\ No newline at end of file
diff --git a/source/System/system.c b/source/System/system.c
index f1dc8d8..cf3810b 100644
--- a/source/System/system.c
+++ b/source/System/system.c
@@ -9,9 +9,11 @@
 #include <stdbool.h>
 #include <stdint.h>
 #include <stdio.h>
+#include <ctype.h>
 
 //Application includes
 #include "eeprom.h"
+#include "soft_timer.h"
 #include "System/system.h"
 
 /*******************************************************************************
@@ -72,5 +74,38 @@ SYSTEM_DATA_t* system_getSys(void)
 // call from the main timer at 100Hz
 void system_monitor(void)
 {
+	// system level monitoring in timer ISR
+}
+
+void system_init(void)
+{
+	memset(&sys, 0, sizeof(SYSTEM_DATA_t));
+
+	sys.systemTime = 0;
+	sys.safeMode = false;
+	sys.usbConnected = false;
+	sys.guiMode = GUI_MODE_NORMAL;
+	sys.activeAccessory = NULL;
+	sys.nextAccessory = NULL;
+	sys.maxPowerLimit = POWER_LIMIT_ALKALINE; // default until battery type determined.
+
+
+    char *ver = SW_VERSION;
+
+	for (int i=0; i < strlen(ver); ++i)
+	{
+		if (isalpha(ver[i]))
+		{
+			sys.isBeta = true;
+			break;
+		}
+	}
+	
+	stimer_init();
+
+	sys.h100HzTimer = stimer_start(  10, TIMER_MODE_PERIODIC, NULL, NULL);
+	sys.h10HzTimer  = stimer_start( 100, TIMER_MODE_PERIODIC, NULL, NULL);
+	sys.h1HzTimer   = stimer_start(1000, TIMER_MODE_PERIODIC, NULL, NULL);
+
 
 }
diff --git a/source/System/system.h b/source/System/system.h
index 8f5bb62..deb6287 100644
--- a/source/System/system.h
+++ b/source/System/system.h
@@ -22,6 +22,8 @@
 #define POWER_LIMIT_ALKALINE	5.0f
 #define POWER_LIMIT_EXT_DC		10.0f
 
+#define FREQ_MAX_NUM	 50		//Max number of frequencies allowed
+
 typedef enum
 {
 	GUI_MODE_NORMAL = 0,
@@ -45,8 +47,6 @@ typedef struct ClampData_s
 } ClampData_t;
 
 
-
-
 typedef enum
 {	USB_CONNECTED = 0,
 	SAFE_MODE,
@@ -91,11 +91,18 @@ typedef struct
 
 	bool status[NUM_STATUS_FLAGS];
 
+	bool isBeta;
+
 	int hSysCheckTimer;
-	int hOneSecondTimer;
+	int h1HzTimer;
+	int h10HzTimer;
+	int h100HzTimer;
 
 	char tmpString[64];
 
+	// TODO: change to FREQUENCY_t*
+	uint32_t frequency;
+
 } SYSTEM_DATA_t;
 
 
@@ -104,5 +111,6 @@ uint32_t SYS_GetLanguage(void);
 SYSTEM_DATA_t* system_getSys(void);
 
 void system_monitor(void);
+void system_init(void);
 
 #endif /* SYSTEM_SYSTEM_H_ */
diff --git a/source/eeprom.c b/source/eeprom.c
index aa71e40..32a7639 100644
--- a/source/eeprom.c
+++ b/source/eeprom.c
@@ -25,10 +25,7 @@
  * Definitions
  ******************************************************************************/
 
-#define EE_BUFFER_SIZE 		(8)		//size of buffer for send/receive 4-byte words
-#define EE_BYTES_PER_WORD	(4)
 
-#define EE_HEADER_NUM_BYTES	(3)		//number of bytes in m95512 header - instruction, addrH, addrL
 
 
 /*******************************************************************************
diff --git a/source/eeprom.h b/source/eeprom.h
index 7aae52c..b0e43cd 100644
--- a/source/eeprom.h
+++ b/source/eeprom.h
@@ -8,6 +8,11 @@
 #ifndef EEPROM_H_
 #define EEPROM_H_
 
+#define EE_BUFFER_SIZE 		(8)		//size of buffer for send/receive 4-byte words
+#define EE_BYTES_PER_WORD	(4)
+
+#define EE_HEADER_NUM_BYTES	(3)		//number of bytes in m95512 header - instruction, addrH, addrL
+
 /*
  * 	CAUTION: avoid writing across a 128 byte page boundary! This will cause rollover per the datasheet section 6.6
  */
diff --git a/source/frq.c b/source/frq.c
index 738bfbb..4f8d9af 100644
--- a/source/frq.c
+++ b/source/frq.c
@@ -205,9 +205,9 @@ void Send_Ctrl_Word(uint16_t Control_Reg, SPI_MODE_t mode)
 {
 //Set the control register Send MS Byte first then LSByte
 
-		 foo[0] = (uint8_t)(Control_Reg >> 8); // Reset bit held
-		 foo[1] = (uint8_t)(Control_Reg & 0x00ff);
-		 SPI0_SendBytes(foo, 2, mode);
+	foo[0] = (uint8_t)(Control_Reg >> 8); // Reset bit held
+	foo[1] = (uint8_t)(Control_Reg & 0x00ff);
+	SPI0_SendBytes(foo, 2, mode);
 
 }
 
@@ -247,7 +247,6 @@ void ClearFreqArray(void)
 	}
 }
 
-#define PIN_OUTPUT()
 void FREQ_Init(void)
 {
 	uint32_t tmp;
@@ -276,8 +275,6 @@ void FREQ_Init(void)
 
 void FREQ_LoadFactoryDefaults(void)
 {
-#if 1  //Use new function so we don't have to specify all the parameters
-
        //These are the FACTORY TEST Frequencies
        FREQ_ClearFrequencies();
        FREQ_AddFrequency(512, 1, 1, FT_ACTIVE);
@@ -288,53 +285,6 @@ void FREQ_LoadFactoryDefaults(void)
        FREQ_AddFrequency(65055, 1, 1, FT_ACTIVE);
        FREQ_AddFrequency(88779, 1, 1, FT_ACTIVE);
 
-#else  //Or we can specify all the parameters
-
-       AddFrequency(98, 0, 0, 0, 0, 0,0,0,0,0);
-       AddFrequency(128, 0, 0, 0, 0, 0,0,0,0,0);
-       AddFrequency(263, 0, 1, 0, 0, 165,0.8396,0.47,0.876923,0);
-       AddFrequency(440, 0, 0, 0, 0, 180,0.8396,0.47,0.876923,0);
-       AddFrequency(440, 220, 0, 0, 0, 180,0.8396,0.47,0.876923,0);
-
-       AddFrequency(512, 0, 1, 0, 0, 180,0.8396,0.47,0.876923,0);
-       AddFrequency(512, 256, 0, 0, 0, 180,0.8396,0.47,0.876923,0);
-
-       AddFrequency(560, 0, 0, 0, 0, 180,0.8396,0.47,0.876923,0);
-       AddFrequency(577, 0, 0, 0, 0, 180,0.8396,0.47,0.876923,0);
-
-       AddFrequency(640, 0, 0, 0, 0,180,0.8396,0.47,0.876923,0);
-       AddFrequency(640, 320, 0, 0, 0, 180,0.8396,0.47,0.876923,0);
-
-       AddFrequency(815, 0, 0, 0, 0, 180,0.8396,0.47,0.876923,0);
-       AddFrequency(870, 0, 1, 0, 0, 180,0.8396,0.47,0.876923,0);
-       AddFrequency(870, 435, 0, 0, 0, 180,0.8396,0.47,0.876923,0);
-
-       AddFrequency(940, 0, 0, 0, 0, 180,0.8105,0.47,0.876923,0);
-       AddFrequency(940, 470, 0, 0, 0, 180,0.8105,0.47,0.876923,0);
-
-       AddFrequency(1024, 0, 0, 0, 0, 180,0.8105,0.49,0.911538,0);
-
-       AddFrequency(1170, 0, 1, 0, 0, 180,0.8105,0.49,0.911538,0);
-       AddFrequency(1170, 585, 0, 0, 0, 180,0.8105,0.49,0.911538,0);
-
-       AddFrequency(3140, 0, 1, 1, MASK_144, 180,0.8634,0.482,0.97083,0);
-       AddFrequency(3140, 1570, 0, 0, 0 , 180,0.8634,0.482,0.97083,0);
-
-       AddFrequency(4096, 0, 1, 0, 0, 180,0.8634,0.482,0.97083,0);
-       AddFrequency(6000, 0, 1, 0, 0, 180,0.8634,0.482,0.97083,0);
-       AddFrequency(8010, 0, 0, 1,MASK_144, 180,0.8976,0.53,1.025,0);
-       AddFrequency(8192, 0, 1, 1,MASK_144, 180,0.8976,0.53,1.025,0);
-       AddFrequency(9820, 0, 1, 0,MASK_144, 180,0.8976,0.53,1.025,0);
-
-       AddFrequency(29433, 0, 1, 1,MASK_58, 180,0.9534,0.69,1.043,0);
-       AddFrequency(32770, 0, 1, 1,MASK_58, 180,0.9581,0.713333333,1.009,0);
-       AddFrequency(44499, 0, 1, 1,MASK_15, 180,1,0.91,1.035,0);
-       AddFrequency(66055, 0, 1, 1,MASK_58, 120,1.215,1.35,1.6375,0);
-       AddFrequency(88779, 0, 1, 1,MASK_15, 120,1.355,1.36,1.8857,0);
-       AddFrequency(99037, 0, 0, 1,MASK_58, 120,1.355,01.36,0,0);
-       AddFrequency(200000, 0, 1, 1,MASK_144, 120,1.355,01.36,7.6,0);
-       AddFrequency(480000, 0, 0, 0,MASK_144, 120,4.111,2.111,7.6,0);
-#endif
 }
 
 
diff --git a/source/init.c b/source/init.c
index 1c9609e..78ce4b0 100644
--- a/source/init.c
+++ b/source/init.c
@@ -99,8 +99,8 @@ void Init_vars()
 	
 	stimer_init();
 
-	sys.hSysCheckTimer = stimer_start(1000, TIMER_MODE_PERIODIC, NULL, NULL);
-	sys.hOneSecondTimer = stimer_start(1000, TIMER_MODE_PERIODIC, NULL, NULL);
+	sys.h10HzTimer = stimer_start(100, TIMER_MODE_PERIODIC, NULL, NULL);
+	sys.h1HzTimer = stimer_start(1000, TIMER_MODE_PERIODIC, NULL, NULL);
 
     Estop_timer = 0;
     Vchktmr = 0;
@@ -165,67 +165,6 @@ void Clear_Flags()
 }
 
 
-void Init_Output(void)
-{
-
-	if(sys.adc.V_CHK < EXCEDED)	// Read external voltage disable if present
-	{
-
-//	Read_Tx_Ports();	// check for what's connected is controlled from timer interrupt
-
-	Init_Mode();			// default induction mode
-
-
-	Update_Min_Frequency();	// determine frequency to be applied from last or if clamp or broadcast mode
-
-	Init_Amplitude();	// set amplitude to minimum
-
-//	Measure_Ohms(); // Done calculate Ohms
-
-//	Check_Taps();		// Done determine optimum taps etc
-
-	}				//
-	else
-	 Estop_Mode(); //
-
-
-}
-void Init_Mode()
-{
-uint8_t i;
-
-	// initialize until E2PROM fitted
-
-	for(i = 0; i <= MODE_MAX_NUM; i++ )
-	{
-		mode_Array[i].Selected = true;
-
-	}
-	mode_Array[BROADCAST].Selected = true; //default BCAST always available
-	mode_Array[PORT2_A].Selected = false;
-	mode_Array[PORT2_B].Selected = false;
-
-
-
-
-
-	Cur_Mode = BROADCAST;
-	Old_Mode = Cur_Mode;
-
-	Read_Tx_Ports();			// now scan external ports
-
-}
-
-void Init_Amplitude(void)	// set amplitude to minimum
-{
-//	if(Cur_Mode != BROADCAST)
-//	{
-		Dds_Pot_Val[0] = 0; // address
-		Dds_Pot_Val[1] = Pot_Value[INIT_PWR_LEVEL]; // data
-		SPI0_SendBytes(Dds_Pot_Val, 2, AMPLITUDE);
-//	}
-
-}
 
 
 void Init_PWM_CLKS(void)
@@ -246,14 +185,6 @@ void Init_Ext_Clk(void)
 
 }
 
-void Init_Int_Clk(void)
-{
-
-
-
-}
-
-
 
 void Init_PSU_Pot(void)		// Set PSU_POT half way approx 23V
 {
@@ -272,100 +203,6 @@ void Init_PSU_Pot(void)		// Set PSU_POT half way approx 23V
 	SPI0_SendBytes(Port_State, 3, EXPANDER);
 
 
-}
-
-void Normal_Init(void)
-{
-
-
-	Power_ON_OFF(ON);	//   Enable_Psu();	Ensure Power supply stays switched on.
-	Select_Estop(ISOLATED);	// Ensure output is ISOLATED from connections
-
-
-	Init_vars();
-
-
-	BL_ReadInfo();
-	EE_LoadData();		//Read saved data
-
-
-	Display_Splash();	//Display splash screen
-	Delay_Ticks(200);	// execute short delay
-	KEY_Init();		//Init keys after splash delay to prevent POWER short press on startup
-
-
-	Init_Mode(); //
-
-	PWM_Setup(15890, 0); // switches off PWM
-
-
-
-	Safety_Check(); // Check all voltages are safe to continue in DC.
-
-
-	Check_Bat_Id(); // Check for Alkaline or Lithium and battery insertion error.
-
-
-
-
-		what_val1=0;
-	what_val2=0;
-		Cur_Mode = PORT2_A;
-
-		init_flag = 0;
-	for( i=0; i < 15; i++);
-	{
-		Delay_Ticks(10);
-		Read_Tx_Ports(); // check for whats plugged in at the ports every 100mS.
-	}
-
-	if ((sys.adc.V_ID2 > 3.0) && (sys.adc.V_ID1 > 3.0))
-	{
-		Cur_Mode = BROADCAST;
-		Port_changed_flag= 1;
-		init_flag = 1;
-	}
-
-	Disconnect(ACCY_PORT_2);
-	Delay_Ticks(30);
-
-	Check_For_Clamp_On_Pwr_Up();
-
-
-
-	Select_Output_Port();
-
-
-	Safety_Check(); //  Second time J.I.C Check all voltages are safe to continue in DC.
-
-
-	Normal_Bypass_Chk();
-
-	old_freq = DUMMY_FRQ; 	//force a frequency on initialization
-	frq_chg_tmr = 0;
-
-	Update_Frequency();
-//	Select_Bypass(OFF);
-
-	// HACK
-	Select_Estop(CONNECTED);	// Ensure output is ISOLATED from connections
-
-	Init_Amplitude();
-}
-
-void Init_Pwr_Level_One(void)
-{
-	if(!Init_Done)
-	{
-		Delay_Ticks(10);	// Wait for Current to catch up
-		inc_pwr();			// Set power level to 1
-		Init_Done = true;
-
-	}
-
-
-
-
 }
 
 
diff --git a/source/init.h b/source/init.h
index 018913f..b468426 100644
--- a/source/init.h
+++ b/source/init.h
@@ -15,7 +15,6 @@
 #define	EXCEDED			500	// High voltage value at port
 
 void Clear_Flags(void);
-void Init_Output(void);
 void Init_Amplitude(void);	// set amplitude to minimum
 void Init_PWM_CLKS(void);			//	select internal or Ext clocks
 void Init_Mode(void);
diff --git a/source/io.c b/source/io.c
index 234ee65..3c10ef3 100644
--- a/source/io.c
+++ b/source/io.c
@@ -12,20 +12,14 @@ void io_expanderSet(uint8_t port, uint8_t pins, bool update)
 {
     _expanderState[port] |= pins;
 
-    if (update)
-    {
-        io_update();
-    }
+    if (update) io_update();
 }
 
 void io_expanderClear(uint8_t port, uint8_t pins, bool update)
 {
     _expanderState[port] &= ~pins;
 
-    if (update)
-    {
-        io_update();
-    }
+    if (update) io_update();
 }
 
 // update I/O expander
@@ -42,4 +36,80 @@ void io_expanderSetSafe(void)
     _expanderState[TOP_SR] = 0;
 
     io_update();
+}
+
+void io_expanderClearAll(bool update)
+{
+    _expanderState[BOT_SR] = 0;
+    _expanderState[MID_SR] = 0;
+    _expanderState[TOP_SR] = 0;
+
+    if (update) io_update();
+}
+
+void io_backlightOn(bool on, bool update)
+{
+	if (on)
+	{
+		EXPANDER_SET(BACKLIGHT_ON, update);
+	}
+	else
+	{
+		EXPANDER_CLEAR(BACKLIGHT_ON, update);
+	}
+}
+
+void io_ampABOn(bool on, bool update)
+{
+	if (on)
+	{
+		EXPANDER_SET(AMP_AB_ON, update);
+	}
+	else
+	{
+		EXPANDER_CLEAR(AMP_AB_ON, update);
+	}
+}
+
+void io_ampDOn(bool on, bool update)
+{
+	if (on)
+	{
+		EXPANDER_SET(AMP_D_EN, update);
+	}
+	else
+	{
+		EXPANDER_CLEAR(AMP_D_EN, update);
+	}
+}
+
+void io_broadcastOn(bool on, bool update)
+{
+	if (on)
+	{
+		EXPANDER_SET(BROADCAST_AMP_EN, NO_UPDATE);
+        EXPANDER_SET(BROADCAST_AMP_PWR, NO_UPDATE);
+	}
+	else
+	{
+		EXPANDER_CLEAR(BROADCAST_AMP_EN, NO_UPDATE);
+        EXPANDER_CLEAR(BROADCAST_AMP_PWR, NO_UPDATE);
+	}
+
+    if (update) io_update();
+}
+
+
+void io_ampPsuOn(bool on, bool update)
+{
+    	if (on)
+	{
+		EXPANDER_CLEAR( _AMP_PSU_ON, NO_UPDATE);
+	}
+	else
+	{
+		EXPANDER_SET(_AMP_PSU_ON, NO_UPDATE);
+	}
+
+    if (update) io_update();
 }
\ No newline at end of file
diff --git a/source/io.h b/source/io.h
index 3453244..c217bbf 100644
--- a/source/io.h
+++ b/source/io.h
@@ -36,6 +36,9 @@ void io_update(void);
 #define _P1(p1, p2) p1
 #define _P2(p1, p2) p2
 
+#define PORT(port_pin) _P1 port_pin
+#define PIN(port_pin)  _P2 port_pin
+
 #define GPIO_WRITE(port_pin, value)    GPIO_PinWrite(GPIO, _P1 port_pin, _P2 port_pin, value )
 #define GPIO_READ(port_pin)           GPIO_PinRead (GPIO, _P1 port_pin, _P2 port_pin)
 
@@ -80,14 +83,38 @@ void io_update(void);
 #define TOP_SR_NC1      0b01000000
 #define TOP_SR_NC2      0b10000000
 
-#define _TAP1_LF_ON      (BOTTOM_SR, TAP_102_LF | TAP_204_LF)
-#define _TAP2_LF_ON      (BOTTOM_SR, TAP_102_LF)
-#define _TAP3_LF_ON      (BOTTOM_SR, TAP_204_LF)
-#define _TAP4_LF_ON      (BOTTOM_SR, 0)
+#define _TAP1_LF_ON     (BOT_SR, TAP_102_LF | TAP_204_LF)
+#define _TAP2_LF_ON     (BOT_SR, TAP_102_LF)
+#define _TAP3_LF_ON     (BOT_SR, TAP_204_LF)
+#define _TAP4_LF_ON     (BOT_SR, 0)
+
+#define BYPASS_ON         (MID_SR   , LF_BYPASS)
+#define BACKLIGHT_ON      (BOT_SR, BKLITE_ON)
+#define AMP_AB_ON     	  (TOP_SR, MUX_AB_AMP)
+#define BROADCAST_AMP_EN  (MID_SR, ANT_AMP_EN)
+#define BROADCAST_AMP_PWR (TOP_SR, ANT_AMP_SW)
+
+#define AMP_D_EN          (MID_SR, DAMP_EN)
+
+#define _AMP_PSU_ON        (BOT_SR, AMP_PSU)
+
 
-#define BYPASS_ON        (MID_SR, LF_BYPASS)
 
 #define EXPANDER_SET(port_pins, update)   io_expanderSet  (_P1 port_pins, _P2 port_pins, update)
 #define EXPANDER_CLEAR(port_pins, update) io_expanderClear(_P1 port_pins, _P2 port_pins, update)  
 
+
+typedef struct 
+{
+   uint32_t port;
+   uint32_t pin;
+} gpio_pin_t;
+
+void io_expanderClearAll(bool update);
+void io_backlightOn(bool on, bool update);
+void io_ampABOn(bool on, bool update);
+void io_ampDOn(bool on, bool update);
+void io_broadcastOn(bool on, bool update);
+void io_ampPsuOn(bool on, bool update);
+
 #endif
diff --git a/source/keys.c b/source/keys.c
index 5633587..54eccd4 100644
--- a/source/keys.c
+++ b/source/keys.c
@@ -19,6 +19,7 @@
 #include "keys.h"
 #include "timer.h"
 #include "io.h"
+#include "System/system.h"
 
 /*******************************************************************************
  * Definitions
@@ -75,6 +76,7 @@ typedef struct
 	uint32_t lastState;				//key on/off last tick
 	uint32_t longPressDetected;		//Long press
 	uint32_t shortPressDetected;	//short press
+	uint32_t pressTime;
 } KEY_DATA_t;
 
 KEY_DATA_t keyData[KEY_NUM];
@@ -86,6 +88,8 @@ extern TIMER_t tmr;
 //TESTING
 static uint32_t keyPressCount = 0;
 
+static SYSTEM_DATA_t *_sys;
+
 /*******************************************************************************
  * Static Function Declarations
  ******************************************************************************/
@@ -129,6 +133,8 @@ void KEY_Init(void)
 	InitKeyData();
 	KEY_ClearKeyPresses();
 	KEY_ClearAll();
+
+	_sys = system_getSys();
 }
 
 //reset all key states and timer values
@@ -312,8 +318,15 @@ void KEY_Update(void)
 		if (!GPIO_PinRead(GPIO, keyData[i].gpioPort, keyData[i].gpioPin))
 		{
 			keyData[i].currentState = 1;
-			keyData[i].tickCount++;
-			if (keyData[i].tickCount == KEY_LONG_PRESS_TICKS)
+
+			if (keyData[i].pressTime == 0)
+			{
+				keyData[i].pressTime = _sys->systemTime;
+			}
+			
+			keyData[i].tickCount = (_sys->systemTime - keyData[i].pressTime);
+
+			if (keyData[i].tickCount >= KEY_LONG_PRESS_TICKS)
 			{
 				//Long press detected
 				key_bits |= ((1 << i) << KEY_LONG_PRESS);//set long press event
@@ -337,6 +350,7 @@ void KEY_Update(void)
 					key_bits |=	((1 << i) << KEY_SHORT_PRESS);//set short press event
 				}
 				keyData[i].tickCount = 0;	//reset tick count
+				keyData[i].pressTime = 0;
 			}
 		}
 	}
diff --git a/source/keys.h b/source/keys.h
index b4a8652..7e04053 100644
--- a/source/keys.h
+++ b/source/keys.h
@@ -35,9 +35,9 @@
 #define KEY_TIMER_PERIOD_MS 		10  //pdMS_TO_TICKS(25)	//25mS
 
 //Key Press Parameters
-#define KEY_SHORT_PRESS_MIN_TICKS	5	//50mS
-#define KEY_SHORT_PRESS_MAX_TICKS	49	//490mS
-#define KEY_LONG_PRESS_TICKS		50	//500mS
+#define KEY_SHORT_PRESS_MIN_TICKS	50	//50mS
+#define KEY_SHORT_PRESS_MAX_TICKS	490	//490mS
+#define KEY_LONG_PRESS_TICKS		500	//500mS
 
 
 //Key Aliases
diff --git a/source/main.c b/source/main.c
index d4714ac..4f3ecfe 100644
--- a/source/main.c
+++ b/source/main.c
@@ -51,6 +51,7 @@
 #include "System/system.h"
 #include "io.h"
 #include "soft_timer.h"
+#include "driver.h"
 
 extern volatile uint8_t BC_Duty_Cycle;
 
@@ -79,7 +80,8 @@ ClampData_t clampData;
 void setSafeMode(bool safe)
 {
 	sys.safeMode = safe;
-	Select_Estop((safe ? ISOLATED : CONNECTED));
+	//Select_Estop((safe ? ISOLATED : CONNECTED));
+	driver_isolateOutput(true);
 
 	if (safe)
 	{
@@ -119,84 +121,55 @@ static void init(void)
 		CLOCK_SetupFROClocking(96000000U); // Set up high frequency FRO output for USB
 		POWER_DisablePD(kPDRUNCFG_PD_USB0_PHY); /*Turn on USB Phy */
 
+		system_init();
 		_firstInit = false;
 	}
 
 	
-    Power_ON_OFF(ON);	//   Enable_Psu();	Ensure Power supply stays switched on.
-    Select_Estop(ISOLATED);	// Ensure output is ISOLATED from connections
-
-   	Init_vars();
+	driver_enablePower(true);
+	driver_isolateOutput(true);
 
     timer_init();
     SPI_Init();
     LCD_Init();
 
     HWF_Init();
-    FREQ_Init();
-    Init_PSU_Pot(); 	// initialize pot.
 
-    Init_Ports();			// Ensure Ports are set to safe mode
+	driver_init();
+	ADC_SysInit();
+    ACCY_Init();
+
+	io_expanderClearAll(false);
+	io_backlightOn(true, NO_UPDATE);
+	io_ampABOn(true, NO_UPDATE);
+	io_update();
 
     BL_ReadInfo();
     EE_LoadData();		//Read saved data
 
+	Display_Splash();	//Display splash screen
 
-    Display_Splash();	//Display splash screen
-    Delay_Ticks(100);	// execute short delay
+	delayms(1000);
 
-	ADC_SysInit();
-    ACCY_Init();
 
     KEY_Init();		//Init keys after splash delay to prevent POWER short press on startup
 
-    //Init_Mode(); //
-
-    PWM_Setup(15890, 0); // switches off PWM
+	driver_setDuty(0);		// switches off PWM
     Delay_Ticks(100);	// execute short delay
 
     //Safety_Check(); // Check all voltages are safe to continue in DC.
 
     Check_Bat_Id(); // Check for Alkaline or Lithium and battery insertion error.
 
-	init_flag = 0;
 
 
-	if ((sys.adc.V_ID2 > 3.0) && (sys.adc.V_ID1 > 3.0))
-	{
-		Cur_Mode = BROADCAST;
-		Port_changed_flag= 1;
-		init_flag = 1;
-	}
-
-	//Disconnect(ACCY_PORT_2);
-	Delay_Ticks(30);
-
-	//Check_For_Clamp_On_Pwr_Up();
-
-	//Select_Output_Port();
-
-    //Safety_Check(); //  Second time J.I.C Check all voltages are safe to continue in DC.
-
-	//Normal_Bypass_Chk();
-
-	old_freq = DUMMY_FRQ; 	//force a frequency on initialization
-	frq_chg_tmr = 0;
-
-	//Update_Frequency();
-
-	// HACK
-	Select_Estop(ISOLATED);	// Restore output.
-
-	Init_Amplitude();
-
 	USB_Init();
 
 	Menu_init();
 
 	// Broadcast accessory is always connected
 	ACCY_Connect(&sys.ports[ACCY_PORT_INDUCTION], ID_BROADCAST);
-	ACCY_setActive(&sys.ports[ACCY_PORT_INDUCTION], CHANNEL_A);
+//	ACCY_setActive(&sys.ports[ACCY_PORT_INDUCTION], CHANNEL_A);
 
 	sys.guiMode = GUI_MODE_NORMAL;
 }
@@ -246,46 +219,33 @@ int main(void)
 			}
 		}
 
-    	Delay_Ticks(1);				//10mS delay
+    	//delayms(10);				//10mS delay
 
-		KEY_Update();
-
-		//Tx_TimeOut();			// Check main transmitter timer
-
-
-		if (stimer_fired(sys.hSysCheckTimer))
+		if (stimer_fired(sys.h100HzTimer))
 		{
-#if 0				
-			System_Check();			// Check all system functions
-			Chk_Gain();
-
-		
-			if(catch_up_flag)
-			{
-				Delay_Ticks(10);
-				Request_Current_Change();	//		Request_Current_Change();
-				catch_up_flag = false;
-			}
-#endif				
-			stimer_clearFired(sys.hSysCheckTimer);
+			KEY_Update();
+			stimer_clearFired(sys.h100HzTimer);
 		}
 
-
-		// one second tasks
-		if (stimer_fired(sys.hOneSecondTimer))
+		// 1Hz tasks
+		if (stimer_fired(sys.h1HzTimer))
 		{
+			// check for power off timeout
+
 			if (sys.usbConnected)
 			{
 				// send diagnostics
 				//sprintf(tmpString, "Hello!");
 				//USB_SendString(tmpString);
 			}
-			stimer_clearFired(sys.hOneSecondTimer);
+			stimer_clearFired(sys.h1HzTimer);
 		}
 
-		// 10Hz
-    	if(tickCount++ % 10 == 0)
+		// 10Hz Tasks
+    	if (stimer_fired(sys.h10HzTimer))
     	{
+			stimer_clearFired(sys.h10HzTimer);
+
 			// check for accessories
     		ACCY_service();
 
@@ -334,8 +294,9 @@ int main(void)
 					{
 						//mode_menu();
 						// test accessory state machine
-						ACCY_next();
+						//ACCY_next();
 
+						ACCY_setActive(&sys.ports[ACCY_PORT_INDUCTION], CHANNEL_A);
 						break;
 					}
 #if 0
diff --git a/source/menu.c b/source/menu.c
index 69846d0..c196f02 100644
--- a/source/menu.c
+++ b/source/menu.c
@@ -34,6 +34,7 @@
 #include "ports.h"
 #include "display.h"
 #include "io.h"
+#include "utils.h"
 
 /*******************************************************************************
  * Definitions
@@ -660,7 +661,7 @@ static void SystemInfoMenu(void)
 							while(KEY_GetDownKeyHeld());
 						}
 
-						Delay_Ticks(10);	//100mS
+						delayms(100);
 					}
 					if((mode == 2) && (up == 2) && (down == 1))
 					{
diff --git a/source/ports.c b/source/ports.c
index cb83e59..f0f2ca7 100644
--- a/source/ports.c
+++ b/source/ports.c
@@ -31,13 +31,12 @@
 #include "pwr_level.h"
 #include "System/system.h"
 #include "usbComms.h"
+#include "driver.h"
 
-ACCESSORY_t accy1;
-ACCESSORY_t accy2;
 uint16_t	Port_timer;
 uint16_t	Taps_adjust_timer;
 extern uint8_t	Port_changed_flag,Init_Done;
-extern uint8_t Port_State[],Cur_Mode,old_freq,Bcast_Pwr_Level,Bcast_LF_Value[],Bcast_HF_Value[],Power_Level;
+extern uint8_t Port_State[],Cur_Mode,old_freq,Bcast_Pwr_Level,Power_Level;
 extern MODE_REC_t mode_Array[MODE_MAX_NUM];
 extern uint8_t frequency,Over_Voltage_Flag;
 extern FREQUENCY_t freqArray[FREQ_MAX_NUM];
@@ -50,6 +49,9 @@ extern SYSTEM_DATA_t sys;
 
 uint8_t whatever;
 
+static uint8_t Bcast_LF_Value[5] = {0,5,10,15,20};
+static uint8_t Bcast_HF_Value[5] = {0,12,25,37,50};
+
 static int handleBroadcast(ACCESSORY_t *accy)
 {
 	switch (accy->state)
@@ -58,8 +60,21 @@ static int handleBroadcast(ACCESSORY_t *accy)
 		{
 			if (accy->initState)
 			{
-				USB_SendString("Broadcast initializing...");
-				accy->stateTimer = sys.systemTime + 2000;
+				// use the current frequency if it is valid, otherwise, set to the lowest valid for broadcast
+				sys.frequency = 3140;
+
+				if(sys.frequency <= 8010)
+					BC_Duty_Cycle = Bcast_LF_Value[0];
+				else
+					BC_Duty_Cycle = Bcast_HF_Value[0];
+
+
+				PWM_Setup(sys.frequency, BC_Duty_Cycle);//freqArray[frequency].frequency1
+
+				driver_broadcastOn(true);
+
+				
+				accy->stateTimer = sys.systemTime + 500;
 			}
 
 			if (sys.systemTime >= accy->stateTimer)
@@ -73,14 +88,25 @@ static int handleBroadcast(ACCESSORY_t *accy)
 		{
 			if (accy->initState)
 			{
-				USB_SendString("Broadcast deinitializing...");
-				accy->stateTimer = sys.systemTime + 2000;
+				//USB_SendString("Broadcast deinitializing...");
+
+				// turn off the amp and antenna
+				driver_broadcastOn(false);
+				delayms(50);
+
+				// turn off PWM
+				driver_setDuty(0);
+
+				// set PSU to minimum
+				driver_setPSUVoltage(V_24V);
+
+				accy->stateTimer = sys.systemTime + 500;
 			}
 
 			if (sys.systemTime >= accy->stateTimer)
 			{
 				accy->state = PORT_STATE_STANDBY;
-				USB_SendString("Broadcast in standby!");
+				//USB_SendString("Broadcast in standby!");
 			}
 
 			break;
@@ -90,7 +116,7 @@ static int handleBroadcast(ACCESSORY_t *accy)
 		{
 			if (accy->initState)
 			{
-				USB_SendString("Broadcast running!");
+				//USB_SendString("Broadcast running!");
 			}
 			break;	
 		}
@@ -223,6 +249,12 @@ void ACCY_service(void)
 	
 }
 
+void ACCY_setActiveChannel(AccyChannelId_t channel)
+{
+	// TODO: handle anything needed for switching here
+	sys.activeAccessory->activeChannel = channel;
+}
+
 void ACCY_next(void)
 {
 	// cycle through accessories and channels to find the next available
@@ -260,13 +292,20 @@ void ACCY_next(void)
 		channel++;
 
 	} while ((port != currentPort) || (channel != currentChannel));
-	
 
 }
 
 void ACCY_setActive(ACCESSORY_t *accy, AccyChannelId_t channel)
 {
 	accy->activeChannel = channel;
+
+	// if just changing channels on the active accessory, don't deinit/init
+	// assume accessory's function will detect the channel change and respond
+	if (accy == sys.activeAccessory)
+	{
+		return;
+	}
+
 	// deinitialize current accessory
 	if (sys.activeAccessory != NULL)
 	{
@@ -382,76 +421,6 @@ static ACCY_ID_t ReadAccessory(uint8_t port)
 	return (ACCY_ID_t)idNumber;
 }
 
-void Disconnect(uint8_t port) // called when you disconnect
-{
-	if (port == ACCY_PORT_1)
-	{
-		accy1.connected = ID_NONE;
-		accy1.consecutiveScans = 0;
-		accy1.isConnected = false;
-
-		//Change Mode if this accessory was selected
-		if(mode_Array[PORT1_A].Selected || mode_Array[PORT1_B].Selected)
-		{
-			//disconnect port 1 from keith's struct
-			mode_Array[PORT1_A].Selected = 0;
-			mode_Array[PORT1_B].Selected = 0;
-
-			if ((Cur_Mode > BROADCAST) && (Cur_Mode ==PORT1_A) || (Cur_Mode == PORT1_B) ) //and change mode
-			    Cur_Mode = Next_Available_Mode(Cur_Mode);		// find next Mode available
-		}
-		else
-		{
-			//disconnect port 1 from keith's struct
-			mode_Array[PORT1_A].Selected = 0;
-			mode_Array[PORT1_B].Selected = 0;
-		}
-	}
-	else	//port 2
-	{
-		accy2.connected = ID_NONE;
-		accy2.consecutiveScans = 0;
-		accy2.isConnected = false;
-
-		//Change Mode if this accessory was selected
-		if(mode_Array[PORT2_A].Selected || mode_Array[PORT2_B].Selected)
-		{
-			//disconnect port 2 from keith's struct
-			mode_Array[PORT2_A].Selected = 0;
-			mode_Array[PORT2_B].Selected = 0;
-
-			if ((Cur_Mode > BROADCAST) && (Cur_Mode == PORT2_A) || (Cur_Mode == PORT2_B) ) //and change mode
-			 //   Cur_Mode = Next_Available_Mode(Cur_Mode);		// find next Mode available
-			{
-				if (mode_Array[PORT1_A].Selected || mode_Array[PORT1_B].Selected) 	//and change mode
-					 Cur_Mode = PORT1_A;
-					else
-					 Cur_Mode = Next_Available_Mode(Cur_Mode);		// find next Mode available
-			}
-				//
-			//			else
-//			  Cur_Mode = 1;
-//			if (Cur_Mode > 0)
-//			{
-//				if (mode_Array[1].Selected || mode_Array[2].Selected) 	//and change mode
-//				Cur_Mode = 1;
-//				else
-//				Cur_Mode = Next_Available_Mode(Cur_Mode);		// find next Mode available
-//			}
-		}
-		else
-		{
-			//disconnect port 1 from keith's struct
-			mode_Array[PORT2_A].Selected = 0;
-			mode_Array[PORT2_B].Selected = 0;
-		}
-	}
-	Port_changed_flag = true; // added 10/2/23
-	Init_Done = false; // 3/20/24 Force a Power Level One change
-
-	//Configure frequency to make sure things are setup correctly
-//	Tx_ConfigureFrequency();
-}
 void  ACCY_Init(void)
 {
 
@@ -467,20 +436,8 @@ void  ACCY_Init(void)
 	sys.ports[ACCY_PORT_1].idVoltage = &sys.adc.V_ID1;	
 	sys.ports[ACCY_PORT_2].idVoltage = &sys.adc.V_ID2;	
 
-	accy1.connected = ID_NONE;
-	accy1.consecutiveScans = 0;
-	accy1.isConnected = false;
-
-	accy2.connected = ID_NONE;
-	accy2.consecutiveScans = 0;
-	accy2.isConnected = false;
-
-	Port_timer = DELAY_100MS;
-	Port_changed_flag = false;
-
-	//Setup accessory for GPIO gain control
-//	InitAccessoryGainGPIO();
 }
+
 void ACCY_Update(void)
 {
 	
diff --git a/source/psu_ctrl.c b/source/psu_ctrl.c
index 5c7326c..4e712ee 100644
--- a/source/psu_ctrl.c
+++ b/source/psu_ctrl.c
@@ -63,55 +63,6 @@ void Get_Max_PSU_Limit(void)					// Set up Maximum voltage for PSU
 }
 
 
-
-void Adjust_Psu(uint8_t direction)
-{
-
-	if(direction == UP)
-	psu_index++;
-	else
-	psu_index--;
-
-}
-
-
-
-void Psu_Match(void)
-{// may need tap check plus current and voltage
-	if(freqArray[frequency].frequency1 <= MAX_DTYPE) //may need current and impedance check
-	{
-		switch (Power_Level)
-		{
-
-			case 0:
-				Set_PSU_Voltage(V_21V);
-			break;
-
-			case 1:
-				Set_PSU_Voltage(V_24V);
-			break;
-
-			case 2:
-				Set_PSU_Voltage(V_27V);
-			break;
-
-			case 3:
-				Set_PSU_Voltage(V_36V);
-			break;
-
-			case 4:
-				Set_PSU_Voltage(V_36V);
-			break;
-
-			default:
-				Set_PSU_Voltage(V_36V);
-			break;
-
-		}
-	}
-}
-
-
 bool Wait_For_Psu(float32_t volt_limit)
 {
 uint16_t escape_count;
diff --git a/source/pwm.h b/source/pwm.h
index 130923b..bb1e48c 100644
--- a/source/pwm.h
+++ b/source/pwm.h
@@ -11,6 +11,8 @@
 #define PWM_EXT2_FREQ_HZ                58983040
 #define PWM_MAX_BROADCAST_ERR_HZ 5.0
 
+#define DEFAULT_PWM_FREQ                15890
+
 typedef enum {
        CS_INTERNAL = 0,
        CS_EXT1,
diff --git a/source/soft_timer.c b/source/soft_timer.c
index 60a1700..217abd2 100644
--- a/source/soft_timer.c
+++ b/source/soft_timer.c
@@ -63,7 +63,8 @@ bool stimer_clearFired(int timer_id)
     }
 }
 
-void stimer_update(void) {
+void stimer_update(void)
+{
     for (int i = 0; i < MAX_TIMERS; ++i) {
         if (timers[i].active && timers[i].remaining_ticks > 0) {
             timers[i].remaining_ticks--;
@@ -83,8 +84,17 @@ void stimer_update(void) {
                 else 
                 {
                     timers[i].active = false;
+                    timers[i].inUse = false;
                 }
             }
         }
     }
 }
+
+// blocking delay
+void stimer_delay(uint32_t ticks)
+{
+    int handle = stimer_start(ticks, TIMER_MODE_ONE_SHOT, 0, 0);
+
+    while (!stimer_fired(handle));
+}
diff --git a/source/soft_timer.h b/source/soft_timer.h
index 9daba8c..d88ca73 100644
--- a/source/soft_timer.h
+++ b/source/soft_timer.h
@@ -33,4 +33,6 @@ bool stimer_clearFired(int timer_id);
 void stimer_update(void);  // Should be called from timer ISR
 bool stimer_is_active(int timer_id);
 
+void stimer_delay(uint32_t ticks);
+
 #endif
diff --git a/source/timer.c b/source/timer.c
index 9ce8695..29db481 100644
--- a/source/timer.c
+++ b/source/timer.c
@@ -175,14 +175,10 @@ uint8_t i;
 
 void Delay_Ticks(uint32_t Delay_Interval)
 {
-	Sys_Timer = Delay_Interval;		// load timer with multiples of 10mS
-
-	while (Sys_Timer > 0);
-
+	stimer_delay(Delay_Interval * 10);
 }
 
 
-
 void Tx_TimeOut(void)
 {
 	if (tmr.autoShutdown != SD_NEVER)
diff --git a/source/utils.c b/source/utils.c
index 5782c5b..f10cb11 100644
--- a/source/utils.c
+++ b/source/utils.c
@@ -29,7 +29,7 @@
 #include "hwFixes.h"
 #include "System\system.h"
 #include "io.h"
-
+#include "soft_timer.h"
 
 //#define	ON	1
 
@@ -801,3 +801,9 @@ void Umag_Patch(void)
 //	Dds_Pot_Val[1] = Pot_Value_AB[Power_Level]; // data
 //
 
+
+
+void delayms(uint32_t msec)
+{
+	stimer_delay(msec);
+}
\ No newline at end of file
diff --git a/source/utils.h b/source/utils.h
index ae88f2a..5bbe3e7 100644
--- a/source/utils.h
+++ b/source/utils.h
@@ -134,6 +134,7 @@ void Umag_Patch(void);
 float32_t Get_Power_Limit(void);
 bool Check_For_Clamp_New();
 
+void delayms(uint32_t msec);
 
 #endif /* UTILS_H_ */