diff --git a/firmware/src/bsp/ain.cpp b/firmware/src/bsp/ain.cpp
index 1d4cc06..bfaac3b 100644
--- a/firmware/src/bsp/ain.cpp
+++ b/firmware/src/bsp/ain.cpp
@@ -9,9 +9,18 @@ using namespace bsp;
 /**** Private constants ****/
 /**** Private variables ****/
 /**** Private function declarations ****/
-
 /**** Public function definitions ****/
-bsp::AnalogIn::AnalogIn(uint8_t adc_ch)
+bsp::AnalogIn::AnalogIn(void)
+{
+	return;
+}
+
+bsp::AnalogIn::~AnalogIn(void)
+{
+	return;
+}
+
+void bsp::AnalogIn::init(uint8_t adc_ch)
 {
 	this->adc_ch = adc_ch;
 	this->mul = DEF_AIN_MUL;
@@ -20,6 +29,7 @@ bsp::AnalogIn::AnalogIn(uint8_t adc_ch)
 	this->last_read = 0;
 }
 
+
 uint16_t bsp::AnalogIn::read(void)
 {
 	//Read ADC
diff --git a/firmware/src/bsp/ain.h b/firmware/src/bsp/ain.h
index 00b90b5..75e8eb0 100644
--- a/firmware/src/bsp/ain.h
+++ b/firmware/src/bsp/ain.h
@@ -12,12 +12,12 @@ static const uint8_t DEF_AIN_DIV = 44;
 static const int16_t DEF_AIN_OFFSET = 0;
 
 class AnalogIn
-{
-	protected:
-	uint8_t adc_ch;
-	
+{	
 	public:
-	AnalogIn(uint8_t adc_ch);
+	AnalogIn(void);
+	~AnalogIn(void);
+	
+	void init(uint8_t adc_ch);
 	
 	uint8_t mul;
 	uint8_t div;
@@ -25,6 +25,11 @@ class AnalogIn
 	uint16_t last_read;
 	
 	uint16_t read(void);
+	
+	#ifndef TESTING
+	protected:
+	#endif
+	uint8_t adc_ch;
 };
 
 /**** Public function declarations ****/
diff --git a/firmware/src/bsp/ain_lpf.cpp b/firmware/src/bsp/ain_lpf.cpp
new file mode 100644
index 0000000..a24f491
--- /dev/null
+++ b/firmware/src/bsp/ain_lpf.cpp
@@ -0,0 +1,52 @@
+/**** Includes ****/
+#include "../utils/utils.h"
+#include "mcu/mcu_hal.h"
+#include "ain_lpf.h"
+
+using namespace bsp;
+
+/**** Private definitions ****/
+/**** Private constants ****/
+/**** Private variables ****/
+/**** Private function declarations ****/
+/**** Public function definitions ****/
+bsp::AnalogInLfp::AnalogInLfp(void)
+{
+	return;
+}
+
+bsp::AnalogInLfp::~AnalogInLfp(void)
+{
+	return;
+}
+
+void bsp::AnalogInLfp::init(uint8_t adc_ch)
+{
+	this->adc_ch = adc_ch;
+	this->mul = DEF_AIN_MUL;
+	this->div = DEF_AIN_DIV;
+	this->offset = DEF_AIN_OFFSET;
+	this->strength = 0;
+	this->last_read = 0;
+	this->last_read_direct = 0;
+}
+
+uint16_t bsp::AnalogInLfp::read(void)
+{
+	//Read ADC
+	uint16_t raw = mcu::adc_read(this->adc_ch);
+	
+	//Convert to mV
+	this->last_read_direct = util::convert_muldivoff(raw, this->mul, this->div, this->offset);
+	
+	// Do filtering
+	uint32_t td0 = ((uint32_t)(255 - this->strength) * this->last_read_direct);
+	uint32_t td1 = ((uint32_t)(this->strength) * this->last_read);
+	uint32_t out = (td0 + td1)/255;
+	
+	this->last_read = util::sat_cast(out);
+	
+	return this->last_read;
+}
+
+/**** Private function definitions ****/
diff --git a/firmware/src/bsp/ain_lpf.h b/firmware/src/bsp/ain_lpf.h
new file mode 100644
index 0000000..6a6609f
--- /dev/null
+++ b/firmware/src/bsp/ain_lpf.h
@@ -0,0 +1,36 @@
+#ifndef ANALOG_IN_LPF_H_
+#define ANALOG_IN_LPF_H_
+
+/**** Includes ****/
+#include <stdint.h>
+#include "ain.h"
+
+namespace bsp {
+
+/**** Public definitions ****/
+class AnalogInLfp : public AnalogIn
+{	
+	public:
+	// New stuff
+	AnalogInLfp(void);
+	~AnalogInLfp(void);
+	
+	void init(uint8_t adc_ch);
+	uint16_t read(void);
+	
+	uint8_t strength;
+	uint16_t last_read_direct;
+	
+	#ifndef TESTING
+	protected:
+	#endif
+};
+
+/**** Public function declarations ****/
+
+#ifdef TESTING
+#endif
+
+} //namespace
+
+#endif /* ANALOG_IN_LPF_H_ */
\ No newline at end of file
diff --git a/firmware/src/bsp/board.cpp b/firmware/src/bsp/board.cpp
index e93755d..6cff7da 100644
--- a/firmware/src/bsp/board.cpp
+++ b/firmware/src/bsp/board.cpp
@@ -1,66 +1,106 @@
 /**** Includes ****/
 #include "../utils/utils.h"
+#include "mcu/mcu_hal.h"
 #include "board.h"
 
+using namespace bsp;
+
 /**** Private definitions ****/
 /**** Private constants ****/
 /**** Private variables ****/
 /**** Private function declarations ****/
-
 /**** Public function definitions ****/
-void board_init(void)
+bsp::Board::Board(void)
 {
-	// MCU setup
-	// ADC clock  must be 50kHz to 200kHz
-	// ADC clock = 8MHz/ADC_DIV
-	// PWM frequncy = 8MHz/(2*TOP*TIM_DIM)
+	return;
+}
+
+bsp::Board::~Board(void)
+{
+	return;
+}
+
+void bsp::Board::init(void)
+{
+	// Controller setup
 	mcu::startupCfg_t mcu_cfg;
-	mcu_cfg.adc_clk = mcu::ADC_DIV64;  //125kHz /13.5 = 9259 samples/s
-	mcu_cfg.pwm_clk = mcu::TIM_DIV1;   // 8MHz
-	mcu_cfg.pwm_top = 1000;            // 4kHz
-	mcu_cfg.pwm_ch1_en = 1;
+	
+	mcu_cfg.adc_clk = mcu::ADC_DIV64; // 8MHz/64=125kHz
+	mcu_cfg.pwm_clk = mcu::TIM_DIV1;  // 8MHz/1 = 8MHz
+	mcu_cfg.pwm_top = 500;            // 8000kHz/500 = 16kHz
+	mcu_cfg.od_common_is_pwm = 1;     // Open-drain common is PWM
 	
 	mcu::startup(&mcu_cfg);
 	
-	// Board setup
-	// Fixed function AIN mV and mA scale
-	dccd_i.mul = 215;
-	dccd_i.div = 22;
-	dccd_i.offset = 0;
-	dccd_i.last_read = 0;
+	// Analog inputs
+	this->out_voltage.init(mcu::ADC_VOUT);
+	this->out_voltage.mul = 20;
+	this->out_voltage.div = 1;
+	this->out_voltage.offset = 0;
 	
-	dccd_u.mul = 20;
-	dccd_u.div = 1;
-	dccd_u.offset = 0;
-	dccd_u.last_read = 0;
+	this->out_current.init(mcu::ADC_IOUT);
+	this->out_current.mul = 215;
+	this->out_current.div = 22;
+	this->out_current.offset = 0;
 	
-	bat_u.mul = 20;
-	bat_u.div = 1;
-	bat_u.offset = 0;
-	bat_u.last_read = 12000;
+	this->battery_voltage.init(mcu::ADC_VBAT);
+	this->battery_voltage.mul = 20;
+	this->battery_voltage.div = 1;
+	this->battery_voltage.offset = 0;
 	
-	bat_i.mul = 235;
-	bat_i.div = 6;
-	bat_i.offset = 0;
-	bat_i.last_read = 0;
+	this->battery_current.init(mcu::ADC_IBAT);
+	this->battery_current.mul = 235;
+	this->battery_current.div = 6;
+	this->battery_current.offset = 0;
+	
+	this->ain1.init(mcu::ADC_AIN1);
+	this->ain2.init(mcu::ADC_AIN2);
+	
+	// Digital inputs
+	this->din1.init(mcu::GPIO_DIN1, 0);
+	this->din2.init(mcu::GPIO_DIN2, 0);
+	this->din3.init(mcu::GPIO_DIN3, 0);
+	this->din4.init(mcu::GPIO_DIN4, 0);
+	this->hvdin1.init(mcu::GPIO_HVDIN1, 1);
+	this->hvdin2.init(mcu::GPIO_HVDIN2, 1);
+	this->hvdin3.init(mcu::GPIO_HVDIN3, 1);
+	
+	this->hvdin3_pull.init(mcu::GPIO_HVDIN3_PULL, 0);
+	this->freq_pull.init(mcu::GPIO_FREQ_PULL, 0);
+	
+	// Open-drain outputs
+	this->od1.init(mcu::GPIO_OD1, 1);
+	this->od2.init(mcu::GPIO_OD2, 1);
+	this->od3.init(mcu::GPIO_OD3, 1);
+	this->od4.init(mcu::GPIO_OD4, 1);
+	this->od5.init(mcu::GPIO_OD5, 1);
+	this->od6.init(mcu::GPIO_OD6, 1);
+	this->od_pwm.init(mcu::PWM_OD, 100);
+	
+	// PWM driver output
+	this->out_pwm.init(mcu::PWM_OUT, 95);
+	this->out_low.init(mcu::GPIO_OUT_LOW, 0);
 }
 
-void board_read(void)
+void bsp::Board::read(void)
 {
-	dccd_i.read();
-	dccd_u.read();
-	bat_u.read();
-	bat_i.read();
-	ain1.read();
-	ain2.read();
+	// Update all analog inputs
+	this->out_voltage.read();
+	this->out_current.read();
+	this->battery_voltage.read();
+	this->battery_current.read();
+	this->ain1.read();
+	this->ain2.read();
 	
-	din1.read();
-	din2.read();
-	din3.read();
-	din4.read();
-	hvdin1.read();
-	hvdin2.read();
-	hvdin3.read();
+	// Update all digital inputs
+	this->din1.read();
+	this->din2.read();
+	this->din3.read();
+	this->din4.read();
+	this->hvdin1.read();
+	this->hvdin2.read();
+	this->hvdin3.read();
 }
 
 /**** Private function definitions ****/
+
diff --git a/firmware/src/bsp/board.h b/firmware/src/bsp/board.h
index ab108f9..665109c 100644
--- a/firmware/src/bsp/board.h
+++ b/firmware/src/bsp/board.h
@@ -1,41 +1,67 @@
-#ifndef BSP_BOARD_H_
-#define BSP_BOARD_H_
+#ifndef UDCCD_BOARD_H_
+#define UDCCD_BOARD_H_
 
 /**** Includes ****/
 #include <stdint.h>
 
-#include "mcu/mcu_hal.h"
 #include "ain.h"
+#include "ain_lpf.h"
 #include "din.h"
 #include "dout.h"
-#include "dio.h"
-#include "halfbridge.h"
-#include "pwm.h"
+#include "pwm_out.h"
 
-static bsp::AnalogIn dccd_i = bsp::AnalogIn(mcu::ADC0);
-static bsp::AnalogIn dccd_u = bsp::AnalogIn(mcu::ADC1);
-static bsp::AnalogIn bat_u = bsp::AnalogIn(mcu::ADC2);
-static bsp::AnalogIn bat_i = bsp::AnalogIn(mcu::ADC3);
-static bsp::Hafbridge hbridge =  bsp::Hafbridge(mcu::PWM0, mcu::GPIO15, 95);
-static bsp::AnalogIn ain1 = bsp::AnalogIn(mcu::ADC5); // mode
-static bsp::AnalogIn ain2 = bsp::AnalogIn(mcu::ADC4); // pot
-static bsp::DigitalIn din1 = bsp::DigitalIn(mcu::GPIO0, 0, bsp::DIN_HIGH); //mode
-static bsp::DigitalIn din2 = bsp::DigitalIn(mcu::GPIO1, 0, bsp::DIN_HIGH); //pot
-static bsp::DigitalIn din3 = bsp::DigitalIn(mcu::GPIO2, 0, bsp::DIN_HIGH); //down
-static bsp::DigitalIn din4 = bsp::DigitalIn(mcu::GPIO3, 0, bsp::DIN_HIGH); //up
-static bsp::DigitalIn hvdin1 = bsp::DigitalIn(mcu::GPIO4, 1, bsp::DIN_LOW); //dimm
-static bsp::DigitalIn hvdin2 = bsp::DigitalIn(mcu::GPIO5, 1, bsp::DIN_LOW); //brakes
-static bsp::DigitalIn hvdin3 = bsp::DigitalIn(mcu::GPIO6, 1, bsp::DIN_LOW); //hbrake
-static bsp::DigitalIO hvdin3_pull = bsp::DigitalIO(mcu::GPIO7, bsp::DIN_HIGH); //hbrake pull
-static bsp::DigitalOut odout1 = bsp::DigitalOut(mcu::GPIO9,  1);
-static bsp::DigitalOut odout2 = bsp::DigitalOut(mcu::GPIO10, 1);
-static bsp::DigitalOut odout3 = bsp::DigitalOut(mcu::GPIO11, 1);
-static bsp::DigitalOut odout4 = bsp::DigitalOut(mcu::GPIO12, 1);
-static bsp::DigitalOut odout5 = bsp::DigitalOut(mcu::GPIO13, 1);
-static bsp::DigitalOut odout6 = bsp::DigitalOut(mcu::GPIO14, 1);
-static bsp::PWMout od_pwm = bsp::PWMout(mcu::PWM1);
+namespace bsp {
 
-void board_init(void);
-void board_read(void);
+/**** Public definitions ****/
+class Board
+{	
+	public:
+	Board(void);
+	~Board(void);
+	
+	void init(void);
+	
+	AnalogIn out_voltage;
+	AnalogIn out_current;
+	AnalogIn battery_voltage;
+	AnalogIn battery_current;
+	AnalogIn ain1;
+	AnalogIn ain2;
+	
+	DigitalIn din1;
+	DigitalIn din2;
+	DigitalIn din3;
+	DigitalIn din4;
+	DigitalIn hvdin1;
+	DigitalIn hvdin2;
+	DigitalIn hvdin3;
+	
+	DigitalOut hvdin3_pull;	
+	DigitalOut freq_pull;
+	
+	DigitalOut od1;
+	DigitalOut od2;
+	DigitalOut od3;
+	DigitalOut od4;
+	DigitalOut od5;
+	DigitalOut od6;
+	PwmOut od_pwm;
+	
+	PwmOut out_pwm;
+	DigitalOut out_low;
+	
+	void read(void);
 
-#endif /* BSP_BOARD_H_ */
\ No newline at end of file
+	#ifndef TESTING
+	protected:
+	#endif
+};
+
+/**** Public function declarations ****/
+
+#ifdef TESTING
+#endif
+
+} //namespace
+
+#endif /* UDCCD_BOARD_H_ */
\ No newline at end of file
diff --git a/firmware/src/bsp/din.cpp b/firmware/src/bsp/din.cpp
index 4ea82d3..dd9b953 100644
--- a/firmware/src/bsp/din.cpp
+++ b/firmware/src/bsp/din.cpp
@@ -9,15 +9,10 @@ using namespace bsp;
 /**** Private constants ****/
 /**** Private variables ****/
 /**** Private function declarations ****/
-
 /**** Public function definitions ****/
-bsp::DigitalIn::DigitalIn(uint8_t gpio_ch, uint8_t inverted, uint8_t init_value)
+bsp::DigitalIn::DigitalIn(void)
 {
-	this->gpio_ch = gpio_ch;
-	this->invert = inverted;
-	
-	if(init_value) this->last_read = DIN_HIGH;
-	else this->last_read = DIN_LOW;
+	return;
 }
 
 bsp::DigitalIn::~DigitalIn(void)
@@ -25,16 +20,29 @@ bsp::DigitalIn::~DigitalIn(void)
 	return;
 }
 
+void bsp::DigitalIn::init(uint8_t gpio_ch, uint8_t inverted)
+{
+	this->gpio_ch = gpio_ch;
+	if(inverted == 0) this->is_inverted = 0;
+	else this->is_inverted = 1;
+	this->last_read = 0;
+}
+
+
 uint8_t bsp::DigitalIn::read(void)
 {
-	uint8_t lvl = mcu::gpio_read(this->gpio_ch);
+	// Read ADC
+	this->last_read = mcu::gpio_read(this->gpio_ch);
 	
-	if(this->invert) lvl = util::invert(lvl);
-	
-	if(lvl>0) this->last_read = DIN_HIGH;
-	else this->last_read = DIN_LOW;
+	// Invert if necessary
+	if(this->is_inverted)
+	{
+		if(this->last_read==0) this->last_read = 1;
+		else this->last_read = 0;
+	};
 	
 	return this->last_read;
 }
 
 /**** Private function definitions ****/
+
diff --git a/firmware/src/bsp/din.h b/firmware/src/bsp/din.h
index 8d8e924..af0080d 100644
--- a/firmware/src/bsp/din.h
+++ b/firmware/src/bsp/din.h
@@ -1,5 +1,5 @@
-#ifndef DIGITAL_INPUT_H_
-#define DIGITAL_INPUT_H_
+#ifndef DIGITAL_IN_H_
+#define DIGITAL_IN_H_
 
 /**** Includes ****/
 #include <stdint.h>
@@ -7,22 +7,23 @@
 namespace bsp {
 
 /**** Public definitions ****/
-const uint8_t DIN_LOW  = 0;
-const uint8_t DIN_HIGH = 1;
-
 class DigitalIn
-{
-	protected:
-	uint8_t gpio_ch;
-	uint8_t invert;
-	
+{	
 	public:
-	DigitalIn(uint8_t gpio_ch, uint8_t inverted, uint8_t init_value);
+	DigitalIn(void);
 	~DigitalIn(void);
 	
+	void init(uint8_t gpio_ch, uint8_t inverted);
+	
 	uint8_t last_read;
 	
 	uint8_t read(void);
+	
+	#ifndef TESTING
+	protected:
+	#endif
+	uint8_t gpio_ch;
+	uint8_t is_inverted;
 };
 
 /**** Public function declarations ****/
@@ -32,4 +33,4 @@ class DigitalIn
 
 } //namespace
 
-#endif /* DIGITAL_INPUT_H_ */
\ No newline at end of file
+#endif /* DIGITAL_IN_H_ */
\ No newline at end of file
diff --git a/firmware/src/bsp/dio.cpp b/firmware/src/bsp/dio.cpp
deleted file mode 100644
index ead2112..0000000
--- a/firmware/src/bsp/dio.cpp
+++ /dev/null
@@ -1,34 +0,0 @@
-/**** Includes ****/
-#include "../utils/utils.h"
-#include "mcu/mcu_hal.h"
-#include "dio.h"
-
-using namespace bsp;
-
-/**** Private definitions ****/
-/**** Private constants ****/
-/**** Private variables ****/
-/**** Private function declarations ****/
-
-/**** Public function definitions ****/
-bsp::DigitalIO::DigitalIO(uint8_t gpio_ch, uint8_t init_value) : DigitalIn(gpio_ch, 0, init_value), DigitalOut(gpio_ch, 0)
-{
-	return;
-}
-
-bsp::DigitalIO::~DigitalIO(void)
-{
-	this->write(DOUT_HIZ);
-}
-
-uint8_t bsp::DigitalIO::is_io_match(void)
-{
-	if(this->last_set == DOUT_HIZ) return 1;
-	
-	uint8_t read_lvl = this->read();
-	
-	if(read_lvl == (uint8_t)this->last_set) return 1;
-	else return 0;
-}
-
-/**** Private function definitions ****/
diff --git a/firmware/src/bsp/dio.h b/firmware/src/bsp/dio.h
deleted file mode 100644
index 0506654..0000000
--- a/firmware/src/bsp/dio.h
+++ /dev/null
@@ -1,32 +0,0 @@
-#ifndef DIGITAL_IO_H_
-#define DIGITAL_IO_H_
-
-/**** Includes ****/
-#include <stdint.h>
-#include "din.h"
-#include "dout.h"
-
-namespace bsp {
-
-/**** Public definitions ****/
-const int8_t DIO_LOW  = 0;
-const int8_t DIO_HIGH = 1;
-const int8_t DIO_HIZ   = -1;
-
-class DigitalIO : public DigitalIn, public DigitalOut
-{
-	public:
-	DigitalIO(uint8_t gpio_ch, uint8_t init_value);
-	~DigitalIO(void);
-	
-	uint8_t is_io_match(void);
-};
-
-/**** Public function declarations ****/
-
-#ifdef TESTING
-#endif
-
-} //namespace
-
-#endif /* DIGITAL_IO_H_ */
\ No newline at end of file
diff --git a/firmware/src/bsp/dout.cpp b/firmware/src/bsp/dout.cpp
index fdb880b..cdb7f07 100644
--- a/firmware/src/bsp/dout.cpp
+++ b/firmware/src/bsp/dout.cpp
@@ -9,44 +9,29 @@ using namespace bsp;
 /**** Private constants ****/
 /**** Private variables ****/
 /**** Private function declarations ****/
-
 /**** Public function definitions ****/
-bsp::DigitalOut::DigitalOut(uint8_t gpio_ch, uint8_t inverted)
+bsp::DigitalOut::DigitalOut(void)
 {
-	this->gpio_ch = gpio_ch;
-	this->invert = inverted;
-	this->write(DOUT_HIZ);
+	return;
 }
 
 bsp::DigitalOut::~DigitalOut(void)
 {
-	this->write(DOUT_HIZ);
+	return;
 }
 
 void bsp::DigitalOut::write(int8_t level)
 {
-	if(level > 0)
+	if(this->is_inverted)
 	{
-		this->last_set = DOUT_HIGH;
-		if(this->invert) mcu::gpio_write(this->gpio_ch, mcu::LEVEL_LOW);
-		else mcu::gpio_write(this->gpio_ch, mcu::LEVEL_HIGH);
-	}
-	else if(level == 0)
-	{
-		this->last_set = DOUT_LOW;
-		if(this->invert) mcu::gpio_write(this->gpio_ch, mcu::LEVEL_HIGH);
-		else mcu::gpio_write(this->gpio_ch, mcu::LEVEL_LOW);
-	}
-	else
-	{
-		this->last_set = DOUT_HIZ;
-		mcu::gpio_write(this->gpio_ch, mcu::LEVEL_HIZ);
-	}
-}
-
-int8_t bsp::DigitalOut::get_set_level(void)
-{
-	return this->last_set;
+		if(level==0) level = 1;
+		else if (level > 0) level  = 0;
+	};
+	
+	mcu::gpio_write(this->gpio_ch, level);
+	
+	this->last_writen = level;
 }
 
 /**** Private function definitions ****/
+
diff --git a/firmware/src/bsp/dout.h b/firmware/src/bsp/dout.h
index 49c696e..36e8d98 100644
--- a/firmware/src/bsp/dout.h
+++ b/firmware/src/bsp/dout.h
@@ -1,29 +1,27 @@
-#ifndef DIGITAL_OUTPUT_H_
-#define DIGITAL_OUTPUT_H_
+#ifndef DIGITAL_OUT_H_
+#define DIGITAL_OUT_H_
 
 /**** Includes ****/
 #include <stdint.h>
+#include "din.h"
 
 namespace bsp {
 
 /**** Public definitions ****/
-const int8_t DOUT_LOW  = 0;
-const int8_t DOUT_HIGH = 1;
-const int8_t DOUT_HIZ = -1;
-
-class DigitalOut
-{
-	protected:
-	uint8_t gpio_ch;
-	uint8_t invert;
-	int8_t last_set;
-	
+class DigitalOut : public DigitalIn
+{	
 	public:
-	DigitalOut(uint8_t gpio_ch, uint8_t inverted);
+	// New or redefined stuff
+	DigitalOut(void);
 	~DigitalOut(void);
 	
+	int8_t last_writen;
+	
 	void write(int8_t level);
-	int8_t get_set_level(void);
+	
+	#ifndef TESTING
+	protected:
+	#endif
 };
 
 /**** Public function declarations ****/
@@ -33,4 +31,4 @@ class DigitalOut
 
 } //namespace
 
-#endif /* DIGITAL_OUTPUT_H_ */
\ No newline at end of file
+#endif /* DIGITAL_OUT_H_ */
\ No newline at end of file
diff --git a/firmware/src/bsp/halfbridge.cpp b/firmware/src/bsp/halfbridge.cpp
deleted file mode 100644
index 23321be..0000000
--- a/firmware/src/bsp/halfbridge.cpp
+++ /dev/null
@@ -1,74 +0,0 @@
-/**** Includes ****/
-#include "../utils/utils.h"
-#include "mcu/mcu_hal.h"
-#include "halfbridge.h"
-
-using namespace bsp;
-
-/**** Private definitions ****/
-/**** Private constants ****/
-/**** Private variables ****/
-/**** Private function declarations ****/
-
-/**** Public function definitions ****/
-bsp::Hafbridge::Hafbridge(uint8_t hs_pwm_ch, uint8_t ls_gpio_ch, uint8_t max_dc)
-{
-	this->pwm_ch  = hs_pwm_ch;	
-	this->gpio_ch = ls_gpio_ch;
-	
-    if(max_dc>100) max_dc = 100;
-	
-	this->max_dc = util::percent_to_16b(max_dc);
-
-	this->disable();
-}
-
-bsp::Hafbridge::~Hafbridge(void)
-{
-	this->last_duty = 0;
-	this->disable();
-}
-
-void bsp::Hafbridge::write(uint16_t dividend)
-{
-	// Limit duty
-	if(dividend > this->max_dc) dividend = this->max_dc;
-	this->last_duty = dividend;
-	
-	if(this->enabled == 0) return;
-	
-	// Set PWM
-	mcu::pwm_write(this->pwm_ch, dividend);
-}
-
-void bsp::Hafbridge::write(uint8_t percent)
-{
-	// Convert to dividend/0xFFFF
-	this->write(util::percent_to_16b(percent));
-}
-
-void bsp::Hafbridge::enable(void)
-{
-	mcu::gpio_write(this->gpio_ch, mcu::LEVEL_HIGH);
-	this->enabled = 1;
-	this->write(this->last_duty);
-}
-
-void bsp::Hafbridge::disable(void)
-{
-	mcu::pwm_write(this->pwm_ch, 0);
-	mcu::gpio_write(this->gpio_ch, mcu::LEVEL_LOW);
-	this->enabled = 0;
-}
-
-uint8_t bsp::Hafbridge::get_set_duty(void)
-{
-	return this->last_duty;
-}
-
-uint8_t bsp::Hafbridge::is_enabled(void)
-{
-	return this->enabled;
-}
-
-/**** Private function definitions ****/
diff --git a/firmware/src/bsp/halfbridge.h b/firmware/src/bsp/halfbridge.h
deleted file mode 100644
index c75a21e..0000000
--- a/firmware/src/bsp/halfbridge.h
+++ /dev/null
@@ -1,38 +0,0 @@
-#ifndef HALFBRIDGE_H_
-#define HALFBRIDGE_H_
-
-/**** Includes ****/
-#include <stdint.h>
-
-namespace bsp {
-
-/**** Public definitions ****/
-class Hafbridge
-{
-	protected:
-	uint8_t pwm_ch;
-	uint8_t gpio_ch;
-	uint16_t last_duty;
-	uint8_t enabled;
-	uint16_t max_dc;
-	
-	public:
-	Hafbridge(uint8_t hs_pwm_ch, uint8_t ls_gpio_ch, uint8_t max_dc);
-	~Hafbridge(void);
-	
-	void write(uint16_t dividend);
-	void write(uint8_t percent);
-	void enable(void);
-	void disable(void);
-	uint8_t get_set_duty(void);
-	uint8_t is_enabled(void);
-};
-
-/**** Public function declarations ****/
-
-#ifdef TESTING
-#endif
-
-} //namespace
-
-#endif /* HALFBRIDGE_H_ */
\ No newline at end of file
diff --git a/firmware/src/bsp/mcu/mcu_hal.h b/firmware/src/bsp/mcu/mcu_hal.h
index 01f9e14..187d17d 100644
--- a/firmware/src/bsp/mcu/mcu_hal.h
+++ b/firmware/src/bsp/mcu/mcu_hal.h
@@ -9,6 +9,9 @@ namespace mcu {
 /**** Public definitions ****/
 /*
 */
+const uint8_t LEVEL_LOW  = 0;
+const uint8_t LEVEL_HIGH = 1;
+const int8_t  LEVEL_HIZ  = -1;
 
 const uint8_t GPIO_DIN1 = 0;
 const uint8_t GPIO_DIN2 = 1;
@@ -27,13 +30,11 @@ const uint8_t GPIO_OD6 = 13;
 const uint8_t GPIO_OUT_LOW = 14;
 const uint8_t GPIO_OUT_HIGH = 15;
 const uint8_t GPIO_OD_PWM = 16;
-const uint8_t GPIO_FREQ_PULL = 17;
-const uint8_t GPIO_FREQ1 = 18;
-const uint8_t GPIO_FREQ2 = 19;
-
-const uint8_t LEVEL_LOW  = 0;
-const uint8_t LEVEL_HIGH = 1;
-const int8_t  LEVEL_HIZ  = -1;
+const uint8_t GPIO_FREQ1 = 17;
+const uint8_t GPIO_FREQ2 = 18;
+const uint8_t GPIO_FREQ_PULL = 19;
+const uint8_t GPIO_TX = 20;
+const uint8_t GPIO_RX = 21;
 
 const uint8_t ADC_IOUT  = 0;	//Output current
 const uint8_t ADC_VOUT  = 1;	//Output voltage
@@ -72,16 +73,22 @@ typedef struct {
 	adcClkDiv_t adc_clk;
 	timerClkDiv_t pwm_clk;
 	uint16_t pwm_top;
-	uint8_t pwm_ch1_en;
+	uint8_t od_common_is_pwm;
 } startupCfg_t;
 
 /**** Public function declarations ****/
 void startup(startupCfg_t* hwCfg);
 
+void rtc_set_calibration(uint16_t coef);
+
 uint8_t gpio_read(uint8_t ch);
 void gpio_write(uint8_t ch, int8_t lvl);
 void gpio_write_pull(uint8_t ch, int8_t lvl);
 
+void adc_start(uint8_t ch);
+uint8_t adc_is_running(void);
+uint8_t adc_is_new(void);
+uint16_t adc_read(void);
 uint16_t adc_read(uint8_t ch);
 
 void pwm_write(uint8_t ch, uint16_t dc);
diff --git a/firmware/src/bsp/mcu/mcu_hal_r8.cpp b/firmware/src/bsp/mcu/mcu_hal_r8.cpp
index 6b2d1de..2056d15 100644
--- a/firmware/src/bsp/mcu/mcu_hal_r8.cpp
+++ b/firmware/src/bsp/mcu/mcu_hal_r8.cpp
@@ -8,6 +8,8 @@ using namespace mcu;
 /**** Private definitions ****/
 /**** Private constants ****/
 /**** Private variables ****/
+static volatile uint16_t rtc_ms = 1000;
+
 /**** Private function declarations ****/
 static uint8_t gpio_read_level(uint8_t pin_reg, uint8_t mask);
 static void pwm_write_ocx(uint8_t ch, uint16_t value);
@@ -34,9 +36,9 @@ void mcu::startup(startupCfg_t* hwCfg)
 	DDRB |= 0x03;   //Set as output
 	
 	// Common OD PWM pin
-	if(hwCfg->pwm_ch1_en) PORTB &= ~0x04; //Set low
-	else PORTB |= 0x04;                   //Set high
-	DDRB |= 0x04;                         //Set as output
+	if(hwCfg->od_common_is_pwm) PORTB &= ~0x04; //Set low
+	else PORTB |= 0x04;                         //Set high
+	DDRB |= 0x04;                               //Set as output
 	
 	// OD control pins
 	PORTD &= ~0x3F; //Set low (off)
@@ -66,6 +68,10 @@ void mcu::startup(startupCfg_t* hwCfg)
 	PORTC &= ~0x30; //Pull-up off
 	DDRC &= ~0x30;  //Set as inputs
 	
+	// Freq-pull control pins
+	PORTD &= ~0x40; //Set low
+	DDRD |= 0x40;   //Set as output
+	
 	//ADC configuration
 	PRR0 &= ~0x01; //Enable ADC power
 	DIDR0 |= 0x0F; //Disable digital inputs, ADC0-ADC3
@@ -80,7 +86,7 @@ void mcu::startup(startupCfg_t* hwCfg)
 	//DCCD and LED PWM configuration
 	PRR0 &= ~0x80;  //Enable Timer1 power
 	TCCR1A = 0xC2; //Connect OC1A, inverted mode
-	if(hwCfg->pwm_ch1_en) TCCR1A |= 0x30; //Connect OC1B, inverted mode
+	if(hwCfg->od_common_is_pwm) TCCR1A |= 0x30; //Connect OC1B, inverted mode
 	TCCR1B = 0x18; //PWM, Phase & Frequency Correct ICR1 top, no clock, WGM:0xE
 	TCCR1C = 0x00;
 	TCNT1 = 0x0000;
@@ -94,7 +100,309 @@ void mcu::startup(startupCfg_t* hwCfg)
 	TCCR1B |= tim1_prescaler;  //Enable timer
 }
 
-// ADC Interface functions 
+void mcu::rtc_set_calibration(uint16_t coef)
+{
+	rtc_ms = coef;
+}
+
+// GPIO interface functions
+uint8_t mcu::gpio_read(uint8_t ch)
+{
+	switch(ch)
+	{
+		case GPIO_DIN1:  // Mode DIN1
+		return gpio_read_level(PINC,0x20);
+		
+		case GPIO_DIN2:  // Pot DIN2
+		return gpio_read_level(PINC,0x10);
+		
+		case GPIO_DIN3:  // Down DIN3
+		return gpio_read_level(PINE,0x02);
+		
+		case GPIO_DIN4:  // Up DIN4
+		return gpio_read_level(PINE,0x08);
+		
+		case GPIO_HVDIN1:  // Dimm DIN5
+		return gpio_read_level(PIND,0x80);
+		
+		case GPIO_HVDIN2:  // Brakes DIN6
+		return gpio_read_level(PINB,0x80);
+		
+		case GPIO_HVDIN3:  // Handbrake DIN7
+		return gpio_read_level(PINB,0x40);
+		
+		case GPIO_HVDIN3_PULL:  // Handbrake pull DIN8
+		return gpio_read_level(PINB,0x20);
+		
+		case GPIO_OD1:  // LED 0
+		return gpio_read_level(PIND,0x01);
+		
+		case GPIO_OD2:  // LED 1
+		return gpio_read_level(PIND,0x02);
+		
+		case GPIO_OD3:  // LED 2
+		return gpio_read_level(PIND,0x04);
+		
+		case GPIO_OD4:  // LED 3
+		return gpio_read_level(PIND,0x08);
+		
+		case GPIO_OD5:  // LED 4
+		return gpio_read_level(PIND,0x10);
+		
+		case GPIO_OD6:  // LED 5
+		return gpio_read_level(PIND,0x20);
+		
+		case GPIO_OUT_LOW:  // DCCD Enable
+		return gpio_read_level(PINB,0x01);	
+		
+		case GPIO_OUT_HIGH:  // DCCD PWM
+		return gpio_read_level(PINB,0x02);
+		
+		case GPIO_OD_PWM:  // LED PWM
+		return gpio_read_level(PINB,0x04);
+		
+		case GPIO_FREQ1:  // Speed 1
+		return gpio_read_level(PINE,0x04);
+		
+		case GPIO_FREQ2:  // Speed 2
+		return gpio_read_level(PINE,0x01);
+		
+		case GPIO_FREQ_PULL:  // Speed-pull
+		return gpio_read_level(PIND,0x40);
+		
+		case GPIO_TX:  // 
+		return gpio_read_level(PINB,0x08);
+		
+		case GPIO_RX:  // 
+		return gpio_read_level(PINB,0x10);	
+		
+		default:
+		return 0;
+	}
+}
+
+void mcu::gpio_write(uint8_t ch, int8_t lvl)
+{
+	switch(ch)
+	{		
+		case GPIO_DIN1:  // Mode DIN1
+		if(lvl>0)
+		{
+			PORTC |= 0x20;
+			DDRC |= 0x20;
+		}
+		else if(lvl<0)
+		{
+			DDRC &= ~0x20;
+			PORTC &= ~0x20;
+		}
+		else
+		{
+			PORTC &= ~0x20;
+			DDRC |= 0x20;
+		}
+		return;
+		
+		case GPIO_DIN2:  // Pot DIN2
+		if(lvl>0)
+		{
+			PORTC |= 0x10;
+			DDRC |= 0x10;
+		}
+		else if(lvl<0)
+		{
+			DDRC &= ~0x10;
+			PORTC &= ~0x10;
+		}
+		else
+		{
+			PORTC &= ~0x10;
+			DDRC |= 0x10;
+		}
+		return;
+		
+		case GPIO_DIN3:  // Down DIN3
+		if(lvl>0)
+		{
+			PORTE |= 0x02;
+			DDRE |= 0x02;
+		}
+		else if(lvl<0)
+		{
+			DDRE &= ~0x02;
+			PORTE &= ~0x02;
+		}
+		else
+		{
+			PORTE &= ~0x02;
+			DDRE |= 0x02;
+		}
+		return;
+		
+		case GPIO_DIN4:  // Up DIN4
+		if(lvl>0)
+		{
+			PORTE |= 0x08;
+			DDRE |= 0x08;
+		}
+		else if(lvl<0)
+		{
+			DDRE &= ~0x08;
+			PORTE &= ~0x08;
+		}
+		else
+		{
+			PORTE &= ~0x08;
+			DDRE |= 0x08;
+		}
+		return;
+		
+		case GPIO_HVDIN3_PULL:  // Handbrake pull DIN
+		if(lvl>0)
+		{
+			PORTB |= 0x20;
+			DDRB |= 0x20;
+		}
+		else if(lvl<0)
+		{
+			DDRB &= ~0x20;
+			PORTB &= ~0x20;
+		}
+		else
+		{
+			PORTB &= ~0x20;
+			DDRB |= 0x20;
+		}
+		return;
+		
+		case GPIO_OD1:  // LED 0
+		if(lvl>0) PORTD |= 0x01;
+		else PORTD &= ~0x01;
+		return;
+		
+		case GPIO_OD2:  // LED 1
+		if(lvl>0) PORTD |= 0x02;
+		else PORTD &= ~0x02;
+		return;
+		
+		case GPIO_OD3:  // LED 2
+		if(lvl>0) PORTD |= 0x04;
+		else PORTD &= ~0x04;
+		return;
+		
+		case GPIO_OD4:  // LED 3
+		if(lvl>0) PORTD |= 0x08;
+		else PORTD &= ~0x08;
+		return;
+		
+		case GPIO_OD5:  // LED 4
+		if(lvl>0) PORTD |= 0x10;
+		else PORTD &= ~0x10;
+		return;
+		
+		case GPIO_OD6:  // LED 5
+		if(lvl>0) PORTD |= 0x20;
+		else PORTD &= ~0x20;
+		return;
+		
+		case GPIO_OUT_LOW:  // DCCD Enable
+		if(lvl>0) PORTB |= 0x01;
+		else PORTB &= ~0x01;
+		return;
+		
+		case GPIO_FREQ_PULL:  // Speed-pull
+		if(lvl>0) PORTD |= 0x40;
+		else PORTD &= ~0x40;
+		return;
+		
+		default:
+		return;
+	}
+}
+
+void mcu::gpio_write_pull(uint8_t ch, int8_t lvl)
+{
+	switch(ch)
+	{
+		case GPIO_DIN1:  // Mode DIN1
+		if(lvl>0) PORTC |= 0x20;
+		else PORTC &= ~0x20;
+		return;
+		
+		case GPIO_DIN2:  // Pot DIN2
+		if(lvl>0) PORTC |= 0x10;
+		else PORTC &= ~0x10;
+		return;
+		
+		case GPIO_DIN3:  // Down DIN3
+		if(lvl>0) PORTE |= 0x02;
+		else PORTE &= ~0x02;
+		return;
+		
+		case GPIO_DIN4:  // Up DIN4
+		if(lvl>0) PORTE |= 0x08;
+		else PORTE &= ~0x08;
+		return;
+		
+		case GPIO_HVDIN1:  // Dimm
+		if(lvl>0) PORTD |= 0x80;
+		else PORTD &= ~0x80;
+		return;
+		
+		case GPIO_HVDIN2:  // Brakes
+		if(lvl>0) PORTB |= 0x80;
+		else PORTB &= ~0x80;
+		return;
+		
+		case GPIO_HVDIN3:  // Handbrake
+		if(lvl>0) PORTB |= 0x40;
+		else PORTB &= ~0x40;
+		return;
+		
+		default:
+		return;
+	}
+}
+
+// ADC interface functions
+void mcu::adc_start(uint8_t ch)
+{
+	// check if already running
+	if(ADCSRA&0x40) return;
+	
+	//check if ADC is enabled
+	if(!(ADCSRA&0x80)) return;
+
+	//Safe guard mux
+	if(ch > 15) return;
+	// Not available channels
+	if((ch > 8) && (ch<14)) return;
+	
+	ADMUX &= ~0x0F;
+	ADMUX |= ch;
+	ADCSRA |= 0x10; // Reset int. flag
+	ADCSRA |= 0x40;
+}
+
+
+uint8_t mcu::adc_is_running(void)
+{
+	if(ADCSRA&0x40) return 1;
+	else return 0;
+}
+
+uint8_t mcu::adc_is_new(void)
+{
+	if(ADCSRA&0x10) return 1;
+	else return 0;
+}
+
+uint16_t mcu::adc_read(void)
+{
+	ADCSRA |= 0x10; // Reset int. flag
+	return ADC;
+}
+
 uint16_t mcu::adc_read(uint8_t ch)
 {
 	//check if ADC is enabled
@@ -112,7 +420,7 @@ uint16_t mcu::adc_read(uint8_t ch)
 	return ADC;
 }
 
-// PWM Timer Interface functions
+// PWM interface functions
 void mcu::pwm_write(uint8_t ch, uint16_t dc)
 {
 	dc = 0xFFFF - dc;
@@ -148,252 +456,7 @@ uint16_t mcu::pwm_read(uint8_t ch)
 	return (uint16_t)temp;
 }
 
-uint8_t mcu::gpio_read(uint8_t ch)
-{
-	switch(ch)
-	{
-		case GPIO0:  // Mode DIN1
-		return gpio_read_level(PINC,0x20);
-		
-		case GPIO1:  // Pot DIN2
-		return gpio_read_level(PINC,0x10);
-		
-		case GPIO2:  // Down DIN3
-		return gpio_read_level(PINE,0x02);
-		
-		case GPIO3:  // Up DIN4
-		return gpio_read_level(PINE,0x08);
-		
-		case GPIO4:  // Dimm DIN5
-		return gpio_read_level(PIND,0x80);
-		
-		case GPIO5:  // Brakes DIN6
-		return gpio_read_level(PINB,0x80);
-		
-		case GPIO6:  // Handbrake DIN7
-		return gpio_read_level(PINB,0x40);
-		
-		case GPIO7:  // Handbrake pull DIN8
-		return gpio_read_level(PINB,0x20);
-		
-		case GPIO8:  // Speed-pull
-		return gpio_read_level(PIND,0x40);
-		
-		case GPIO9:  // LED 0
-		return gpio_read_level(PIND,0x01);
-		
-		case GPIO10:  // LED 1
-		return gpio_read_level(PIND,0x02);
-		
-		case GPIO11:  // LED 2
-		return gpio_read_level(PIND,0x04);
-		
-		case GPIO12:  // LED 3
-		return gpio_read_level(PIND,0x08);
-		
-		case GPIO13:  // LED 4
-		return gpio_read_level(PIND,0x10);
-		
-		case GPIO14:  // LED 5
-		return gpio_read_level(PIND,0x20);
-		
-		case GPIO15:  // DCCD Enable
-		return gpio_read_level(PINB,0x01);	
-		
-		case GPIO16:  // DCCD PWM
-		return gpio_read_level(PINB,0x02);
-		
-		case GPIO17:  // LED PWM
-		return gpio_read_level(PINB,0x04);	
-		
-		default:
-		return 0;
-	}
-}
-
-void mcu::gpio_write(uint8_t ch, int8_t lvl)
-{
-	switch(ch)
-	{		
-		case GPIO0:  // Mode DIN1
-		if(lvl>0)
-		{
-			PORTC |= 0x20;
-			DDRC |= 0x20;
-		}
-		else if(lvl<0)
-		{
-			DDRC &= ~0x20;
-			PORTC &= ~0x20;
-		}
-		else
-		{
-			PORTC &= ~0x20;
-			DDRC |= 0x20;
-		}
-		return;
-		
-		case GPIO1:  // Pot DIN2
-		if(lvl>0)
-		{
-			PORTC |= 0x10;
-			DDRC |= 0x10;
-		}
-		else if(lvl<0)
-		{
-			DDRC &= ~0x10;
-			PORTC &= ~0x10;
-		}
-		else
-		{
-			PORTC &= ~0x10;
-			DDRC |= 0x10;
-		}
-		return;
-		
-		case GPIO2:  // Down DIN3
-		if(lvl>0)
-		{
-			PORTE |= 0x02;
-			DDRE |= 0x02;
-		}
-		else if(lvl<0)
-		{
-			DDRE &= ~0x02;
-			PORTE &= ~0x02;
-		}
-		else
-		{
-			PORTE &= ~0x02;
-			DDRE |= 0x02;
-		}
-		return;
-		
-		case GPIO3:  // Up DIN4
-		if(lvl>0)
-		{
-			PORTE |= 0x08;
-			DDRE |= 0x08;
-		}
-		else if(lvl<0)
-		{
-			DDRE &= ~0x08;
-			PORTE &= ~0x08;
-		}
-		else
-		{
-			PORTE &= ~0x08;
-			DDRE |= 0x08;
-		}
-		return;
-		
-		case GPIO7:  // Handbrake pull DIN
-		if(lvl>0)
-		{
-			PORTB |= 0x20;
-			DDRB |= 0x20;
-		}
-		else if(lvl<0)
-		{
-			DDRB &= ~0x20;
-			PORTB &= ~0x20;
-		}
-		else
-		{
-			PORTB &= ~0x20;
-			DDRB |= 0x20;
-		}
-		return;
-		
-		case GPIO8:  // Speed-pull
-		if(lvl>0) PORTD |= 0x40;
-		else PORTD &= ~0x40;
-		return;
-		
-		case GPIO9:  // LED 0
-		if(lvl>0) PORTD |= 0x01;
-		else PORTD &= ~0x01;
-		return;
-		
-		case GPIO10:  // LED 1
-		if(lvl>0) PORTD |= 0x02;
-		else PORTD &= ~0x02;
-		return;
-		
-		case GPIO11:  // LED 2
-		if(lvl>0) PORTD |= 0x04;
-		else PORTD &= ~0x04;
-		return;
-		
-		case GPIO12:  // LED 3
-		if(lvl>0) PORTD |= 0x08;
-		else PORTD &= ~0x08;
-		return;
-		
-		case GPIO13:  // LED 4
-		if(lvl>0) PORTD |= 0x10;
-		else PORTD &= ~0x10;
-		return;
-		
-		case GPIO14:  // LED 5
-		if(lvl>0) PORTD |= 0x20;
-		else PORTD &= ~0x20;
-		return;
-		
-		case GPIO15:  // DCCD Enable
-		if(lvl>0) PORTB |= 0x01;
-		else PORTB &= ~0x01;
-		return;
-		
-		default:
-		return;
-	}
-}
-
-void mcu::gpio_write_pull(uint8_t ch, int8_t lvl)
-{
-	switch(ch)
-	{
-		case GPIO0:  // Mode DIN1
-		if(lvl>0) PORTC |= 0x20;
-		else PORTC &= ~0x20;
-		return;
-		
-		case GPIO1:  // Pot DIN2
-		if(lvl>0) PORTC |= 0x10;
-		else PORTC &= ~0x10;
-		return;
-		
-		case GPIO2:  // Down DIN3
-		if(lvl>0) PORTE |= 0x02;
-		else PORTE &= ~0x02;
-		return;
-		
-		case GPIO3:  // Up DIN4
-		if(lvl>0) PORTE |= 0x08;
-		else PORTE &= ~0x08;
-		return;
-		
-		case GPIO4:  // Dimm
-		if(lvl>0) PORTD |= 0x80;
-		else PORTD &= ~0x80;
-		return;
-		
-		case GPIO5:  // Brakes
-		if(lvl>0) PORTB |= 0x80;
-		else PORTB &= ~0x80;
-		return;
-		
-		case GPIO6:  // Handbrake
-		if(lvl>0) PORTB |= 0x40;
-		else PORTB &= ~0x40;
-		return;
-		
-		default:
-		return;
-	}
-}
-
+// EEPROM interface functions
 uint8_t mcu::eeprom_read8b(uint16_t address)
 {
 	return eeprom_read_byte((uint8_t*)address);
@@ -435,11 +498,11 @@ static void pwm_write_ocx(uint8_t ch, uint16_t value)
 {
 	switch(ch)
 	{
-		case PWM0:
+		case PWM_OUT:
 		OCR1A = value;
 		return;
 		
-		case PWM1:
+		case PWM_OD:
 		OCR1B = value;
 		return;
 		
@@ -452,10 +515,10 @@ static uint16_t pwm_read_ocx(uint8_t ch)
 {
 	switch(ch)
 	{
-		case PWM0:
+		case PWM_OUT:
 		return OCR1A;
 		
-		case PWM1:
+		case PWM_OD:
 		return OCR1B ;
 		
 		default:
diff --git a/firmware/src/bsp/pwm.cpp b/firmware/src/bsp/pwm.cpp
deleted file mode 100644
index 32db2c5..0000000
--- a/firmware/src/bsp/pwm.cpp
+++ /dev/null
@@ -1,40 +0,0 @@
-/**** Includes ****/
-#include "../utils/utils.h"
-#include "mcu/mcu_hal.h"
-#include "pwm.h"
-
-using namespace bsp;
-
-/**** Private definitions ****/
-/**** Private constants ****/
-/**** Private variables ****/
-/**** Private function declarations ****/
-
-/**** Public function definitions ****/
-bsp::PWMout::PWMout(uint8_t pwm_ch)
-{
-	this->pwm_ch = pwm_ch;
-	this->write(0);
-}
-
-bsp::PWMout::~PWMout(void)
-{
-	this->write(0);
-}
-
-void bsp::PWMout::write(uint8_t duty)
-{
-	// Convert percent to 16b duty cycle
-	uint16_t dc = util::percent_to_16b(duty);
-	
-	// Set PWM
-	mcu::pwm_write(this->pwm_ch, dc);
-	this->last_duty = duty;
-}
-
-uint8_t bsp::PWMout::get_set_duty(void)
-{
-	return this->last_duty;
-}
-
-/**** Private function definitions ****/
diff --git a/firmware/src/bsp/pwm.h b/firmware/src/bsp/pwm.h
deleted file mode 100644
index c5b6cef..0000000
--- a/firmware/src/bsp/pwm.h
+++ /dev/null
@@ -1,31 +0,0 @@
-#ifndef PWM_H_
-#define PWM_H_
-
-/**** Includes ****/
-#include <stdint.h>
-
-namespace bsp {
-
-/**** Public definitions ****/
-class PWMout
-{
-	protected:
-	uint8_t pwm_ch;
-	uint8_t last_duty;
-	
-	public:
-	PWMout(uint8_t pwm_ch);
-	~PWMout(void);
-	
-	void write(uint8_t duty);
-	uint8_t get_set_duty(void);
-};
-
-/**** Public function declarations ****/
-
-#ifdef TESTING
-#endif
-
-} //namespace
-
-#endif /* PWM_H_ */
\ No newline at end of file
diff --git a/firmware/src/bsp/pwm_out.cpp b/firmware/src/bsp/pwm_out.cpp
new file mode 100644
index 0000000..72c34ab
--- /dev/null
+++ b/firmware/src/bsp/pwm_out.cpp
@@ -0,0 +1,56 @@
+/**** Includes ****/
+#include "../utils/utils.h"
+#include "mcu/mcu_hal.h"
+#include "pwm_out.h"
+
+using namespace bsp;
+
+/**** Private definitions ****/
+/**** Private constants ****/
+/**** Private variables ****/
+/**** Private function declarations ****/
+
+/**** Public function definitions ****/
+bsp::PwmOut::PwmOut(void)
+{
+	return;
+}
+
+bsp::PwmOut::~PwmOut(void)
+{
+	this->last_duty = 0;
+}
+
+void bsp::PwmOut::init(uint8_t pwm_ch, uint8_t max_dc)
+{
+	this->pwm_ch  = pwm_ch;
+	
+	this->last_duty = 0;
+	
+	if(max_dc>100) max_dc = 100;
+	
+	this->max_dc = util::percent_to_16b(max_dc);
+}
+
+void bsp::PwmOut::write(uint16_t numerator)
+{
+	// Update target
+	if(numerator > this->max_dc) numerator = this->max_dc;
+	this->last_duty = numerator;
+	
+	// Set PWM
+	mcu::pwm_write(this->pwm_ch, numerator);
+}
+
+void bsp::PwmOut::write(uint8_t percent)
+{
+	// Convert to numerator/0xFFFF
+	this->write(util::percent_to_16b(percent));
+}
+
+uint16_t bsp::PwmOut::get_set_duty(void)
+{
+	return this->last_duty;
+}
+
+/**** Private function definitions ****/
diff --git a/firmware/src/bsp/pwm_out.h b/firmware/src/bsp/pwm_out.h
new file mode 100644
index 0000000..cdcfec1
--- /dev/null
+++ b/firmware/src/bsp/pwm_out.h
@@ -0,0 +1,37 @@
+#ifndef PWM_OUT_H_
+#define PWM_OUT_H_
+
+/**** Includes ****/
+#include <stdint.h>
+
+namespace bsp {
+
+/**** Public definitions ****/
+class PwmOut
+{
+	public:
+	PwmOut(void);
+	~PwmOut(void);
+	
+	void init(uint8_t pwm_ch, uint8_t max_dc);
+	
+	void write(uint16_t numerator);
+	void write(uint8_t percent);
+	uint16_t get_set_duty(void);
+	
+	#ifndef TESTING
+	protected:
+	#endif
+	uint8_t pwm_ch;
+	uint16_t last_duty;
+	uint16_t max_dc;
+};
+
+/**** Public function declarations ****/
+
+#ifdef TESTING
+#endif
+
+} //namespace
+
+#endif /* PWM_OUT_H_ */
\ No newline at end of file
diff --git a/firmware/src/hw/button.cpp b/firmware/src/hw/button.cpp
index e1153c2..f847af7 100644
--- a/firmware/src/hw/button.cpp
+++ b/firmware/src/hw/button.cpp
@@ -8,25 +8,10 @@ using namespace hw;
 /**** Private constants ****/
 /**** Private variables ****/
 /**** Private function declarations ****/
-
 /**** Public function definitions ****/
-hw::Button::Button(bsp::DigitalIn* din_ch, uint8_t act_lvl, uint8_t dbnc_lim, uint8_t init_state)
+hw::Button::Button(void)
 {
-	this->din_ch = din_ch;
-	
-	if(act_lvl) this->act_lvl = bsp::DIN_HIGH;
-	else this->act_lvl = bsp::DIN_LOW;
-	
-	this->dbnc_cnter = 0;
-	this->dbnc_lim = dbnc_lim;
-	
-	if(init_state) this->state = BUTTON_ON;
-	else this->state = BUTTON_OFF;
-	
-	this->time = 0;
-	this->is_new = 0;
-	
-	this->hold_time = 0;
+	return;
 }
 
 hw::Button::~Button(void)
@@ -34,48 +19,67 @@ hw::Button::~Button(void)
 	return;
 }
 
-uint8_t hw::Button::update(void)
+void hw::Button::init(bsp::DigitalIn* din_ch, uint8_t act_lvl, util::VCounter* timer, uint16_t dbnc_lim)
 {
-	// Read din level
+	this->din_ch = din_ch;
+	this->timer = timer;
+	
+	if(act_lvl) this->act_lvl = 1;
+	else this->act_lvl = 0;
+	
+	this->state_start_ts = 0;
+	this->dbnc_ts = 0;
+	this->dbnc_lim = dbnc_lim;
+	
+	this->state = BUTTON_OFF;
+	this->is_new = 0;
+}
+
+uint8_t hw::Button::process(void)
+{
+	// Read din
+	if(this->update_din) this->din_ch->read();
+	
+	// Get last read level
 	uint8_t lvl = this->din_ch->last_read;
 	
-	// Increase state counter
-	this->time = util::sat_add(this->time, 1);
-	
-	// Repeat new flag after hold time
-	if((this->state == BUTTON_ON)&&(this->time > this->hold_time)&&(this->hold_time > 0))
-	{
-		this->time = 0;
-		this->is_new = 1;
-	};
-	
 	// Determine next state
 	uint8_t next_state = BUTTON_OFF;
 	if(lvl==this->act_lvl) next_state = BUTTON_ON;
 	
 	// Advance debounce sample counter
-	if(next_state != this->state) this->dbnc_cnter++;
-	else this->dbnc_cnter = 0;
-	
-	// Check for debounce end
-	if(this->dbnc_cnter < this->dbnc_lim) return this->state;
-	
-	// Debounce end. Apply new state.
-	this->state = next_state;
-	this->time = 0;
-	this->is_new = 1;
-	this->dbnc_cnter = 0;
+	uint16_t ts_now = this->timer->read();
 	
+	if(next_state != this->state)
+	{
+		if(this->dbnc_ts == 0) this->dbnc_ts = ts_now;
+		uint16_t td = util::time_delta(this->dbnc_ts, ts_now);
+		uint32_t td_ms = this->timer->convert_ms(td);
+		// Check for debounce end
+		if(td_ms >= this->dbnc_lim) 
+		{
+			// Debounce end. Apply new state.
+			this->dbnc_ts = 0;
+			this->state = next_state;
+			this->state_start_ts = ts_now;
+			this->is_new = 1;
+		};
+	}
+	else this->dbnc_ts = 0;
+
 	return this->state;
 }
 
-uint8_t hw::Button::force_update(void)
+uint8_t hw::Button::force_read(void)
 {
-	// Read din level
-	uint8_t lvl = this->din_ch->read();
+	// Read din
+	if(this->update_din) this->din_ch->read();
+	
+	// Get last read level
+	uint8_t lvl = this->din_ch->last_read;
 	
 	// Cancels active debounce
-	this->dbnc_cnter = 0;
+	this->dbnc_ts = 0;
 	
 	// Determine next state
 	uint8_t next_state = BUTTON_OFF;
@@ -83,12 +87,30 @@ uint8_t hw::Button::force_update(void)
 	
 	if(next_state != this->state)
 	{
+		this->state_start_ts = this->timer->read();
 		this->state = next_state;
-		this->time = 0;
 		this->is_new = 1;
 	};
 	
 	return this->state;
 }
 
+uint32_t hw::Button::time_read(void)
+{
+	uint16_t ts_now = this->timer->read();
+	uint16_t td = util::time_delta(this->state_start_ts, ts_now);
+	return this->timer->convert_ms(td);
+}
+
+void hw::Button::time_reset(void)
+{
+	this->state_start_ts = this->timer->read();
+}
+
+uint32_t hw::Button::time_read_max(void)
+{
+	uint16_t ts_max = this->timer->read_top();
+	return this->timer->convert_ms(ts_max);
+}
+
 /**** Private function definitions ****/
diff --git a/firmware/src/hw/button.h b/firmware/src/hw/button.h
index e0f59e1..3be0c0a 100644
--- a/firmware/src/hw/button.h
+++ b/firmware/src/hw/button.h
@@ -1,42 +1,49 @@
-#ifndef BUTTON_H_
-#define BUTTON_H_
+#ifndef BUTTONS_H_
+#define BUTTONS_H_
 
 /**** Includes ****/
 #include <stdint.h>
-#include "../bsp/din.h"
+#include "../utils/vcounter.h"
+#include "../bsp/board.h"
 
 namespace hw {
 
 /**** Public definitions ****/
-const uint8_t BUTTON_OFF  = 0;
-const uint8_t BUTTON_ON = 1;
+const uint8_t BUTTON_OFF = 0;
+const uint8_t BUTTON_ON  = 1;
 
 class Button
 {
-	protected:
-	bsp::DigitalIn* din_ch;
-	uint8_t act_lvl;
-	uint8_t dbnc_cnter;
-	
 	public:
-	Button(bsp::DigitalIn* din_ch, uint8_t act_lvl, uint8_t dbnc_lim, uint8_t init_state);
+	Button(void);
 	~Button(void);
 	
 	uint8_t state;
-	uint16_t time;
-	uint8_t dbnc_lim;
+	uint16_t dbnc_lim;
 	uint8_t is_new;
-	uint16_t hold_time;
+	uint8_t update_din;
 	
-	uint8_t update(void);
-	uint8_t force_update(void);
+	void init(bsp::DigitalIn* din_ch, uint8_t act_lvl, util::VCounter* timer, uint16_t dbnc_lim);
+	uint8_t process(void);
+	uint8_t force_read(void);
+	uint32_t time_read(void);
+	void time_reset(void);
+	uint32_t time_read_max(void);
+	
+	#ifndef TESTING
+	protected:
+	#endif
+	bsp::DigitalIn* din_ch;
+	util::VCounter* timer;
+	uint8_t act_lvl;
+	uint16_t state_start_ts;
+	uint16_t dbnc_ts;
 };
 
 /**** Public function declarations ****/
-
 #ifdef TESTING
 #endif
 
 } //namespace
 
-#endif /* BUTTON_H_ */
\ No newline at end of file
+#endif /* BUTTONS_H_ */
\ No newline at end of file
diff --git a/firmware/src/hw/cc_output.cpp b/firmware/src/hw/cc_output.cpp
deleted file mode 100644
index bd3e713..0000000
--- a/firmware/src/hw/cc_output.cpp
+++ /dev/null
@@ -1,41 +0,0 @@
-/**** Includes ****/
-#include "../utils/utils.h"
-#include "cc_output.h"
-
-using namespace hw;
-
-/**** Private definitions ****/
-/**** Private constants ****/
-/**** Private variables ****/
-/**** Private function declarations ****/
-
-/**** Public function definitions ****/
-hw::CCoutput::CCoutput(bsp::Hafbridge* hbridge, bsp::AnalogIn* supply_u, bsp::AnalogIn* out_u, bsp::AnalogIn* out_i) : CVoutput(hbridge, supply_u)
-{
-	this->out_voltage = out_u;
-	this->out_currnet = out_i;
-	
-	this->out_impedance = 0xFFFF;
-	this->target_voltage = 0;
-}
-
-void hw::CCoutput::update(void)
-{
-	// Calculate output impedance
-	if((this->out_currnet == 0)||(this->out_voltage->last_read == 0)) this->out_impedance = 0xFFFF;
-	else
-	{
-		this->out_impedance = util::sat_div_kilo(this->out_voltage->last_read, this->out_currnet->last_read);
-	}
-	
-	// Check target
-	if((this->target < this->min_out)&&(this->target > 0)) this->target = this->min_out;
-	
-	// Convert target current to voltage
-	this->target_voltage = util::sat_mul_kilo(this->target, this->out_impedance);
-	
-	// Set output
-	this->hbridge->write(util::sat_ratio(this->target_voltage, this->supply->last_read));
-}
-
-/**** Private function definitions ****/
diff --git a/firmware/src/hw/cc_output.h b/firmware/src/hw/cc_output.h
deleted file mode 100644
index fee738f..0000000
--- a/firmware/src/hw/cc_output.h
+++ /dev/null
@@ -1,35 +0,0 @@
-#ifndef CONST_CURRENT_OUTPUT_H_
-#define CONST_CURRENT_OUTPUT_H_
-
-/**** Includes ****/
-#include <stdint.h>
-#include "../bsp/ain.h"
-#include "cv_output.h"
-
-namespace hw {
-
-/**** Public definitions ****/
-
-class CCoutput : public CVoutput
-{
-	protected:
-	bsp::AnalogIn* out_voltage;
-	bsp::AnalogIn* out_currnet;
-	
-	public:
-	CCoutput(bsp::Hafbridge* hbridge, bsp::AnalogIn* supply_u, bsp::AnalogIn* out_u, bsp::AnalogIn* out_i);
-	
-	void update(void);
-	
-	uint16_t out_impedance;
-	uint16_t target_voltage;
-};
-
-/**** Public function declarations ****/
-
-#ifdef TESTING
-#endif
-
-} //namespace
-
-#endif /* CONST_VOLTAGE_OUTPUT_H_ */
\ No newline at end of file
diff --git a/firmware/src/hw/cv_output.cpp b/firmware/src/hw/cv_output.cpp
deleted file mode 100644
index 8913c9a..0000000
--- a/firmware/src/hw/cv_output.cpp
+++ /dev/null
@@ -1,53 +0,0 @@
-/**** Includes ****/
-#include "../utils/utils.h"
-#include "cv_output.h"
-
-using namespace hw;
-
-/**** Private definitions ****/
-/**** Private constants ****/
-/**** Private variables ****/
-/**** Private function declarations ****/
-
-/**** Public function definitions ****/
-hw::CVoutput::CVoutput(bsp::Hafbridge* hbridge, bsp::AnalogIn* supply_u)
-{
-	this->hbridge = hbridge;
-	this->supply = supply_u;
-	this->target = 0;
-	this->min_out = 0;
-	this->hbridge->disable();
-}
-
-hw::CVoutput::~CVoutput(void)
-{
-	this->hbridge->write((uint16_t)0x0000);
-	this->hbridge->disable();
-	return;
-}
-
-void hw::CVoutput::update(void)
-{
-	// Check target	
-	if((this->target < this->min_out)&&(this->target > 0)) this->target = this->min_out;
-	
-	// Set output
-	this->hbridge->write(util::sat_ratio(this->target, this->supply->last_read));
-}
-
-void hw::CVoutput::enable(void)
-{
-	this->hbridge->enable();
-}
-
-void hw::CVoutput::disable(void)
-{
-	this->hbridge->disable();	
-}
-
-uint8_t hw::CVoutput::is_enabled(void)
-{
-	return this->hbridge->is_enabled();
-}
-
-/**** Private function definitions ****/
diff --git a/firmware/src/hw/cv_output.h b/firmware/src/hw/cv_output.h
deleted file mode 100644
index 03de3a9..0000000
--- a/firmware/src/hw/cv_output.h
+++ /dev/null
@@ -1,39 +0,0 @@
-#ifndef CONST_VOLTAGE_OUTPUT_H_
-#define CONST_VOLTAGE_OUTPUT_H_
-
-/**** Includes ****/
-#include <stdint.h>
-#include "../bsp/ain.h"
-#include "../bsp/halfbridge.h"
-
-namespace hw {
-
-/**** Public definitions ****/
-
-class CVoutput
-{
-	protected:
-	bsp::Hafbridge* hbridge;
-	bsp::AnalogIn* supply;
-	
-	public:
-	CVoutput(bsp::Hafbridge* hbridge, bsp::AnalogIn* supply_u);
-	~CVoutput(void);
-	
-	uint16_t target;
-	uint16_t min_out;
-	
-	void update(void);
-	void enable(void);
-	void disable(void);
-	uint8_t is_enabled(void);
-};
-
-/**** Public function declarations ****/
-
-#ifdef TESTING
-#endif
-
-} //namespace
-
-#endif /* CONST_VOLTAGE_OUTPUT_H_ */
\ No newline at end of file
diff --git a/firmware/src/hw/dccd_hw.cpp b/firmware/src/hw/dccd_hw.cpp
new file mode 100644
index 0000000..fef1c58
--- /dev/null
+++ b/firmware/src/hw/dccd_hw.cpp
@@ -0,0 +1,110 @@
+/**** Includes ****/
+#include "../utils/utils.h"
+#include "../bsp/mcu/mcu_hal.h"
+#include "dccd_hw.h"
+
+using namespace hw;
+
+/**** Private definitions ****/
+/**** Private constants ****/
+/**** Private variables ****/
+/**** Private function declarations ****/
+/**** Public function definitions ****/
+hw::DccdHw::DccdHw(void)
+{
+	return;
+}
+
+hw::DccdHw::~DccdHw(void)
+{
+	return;
+}
+
+void hw::DccdHw::init(dccdHwCfg_t* cfg)
+{
+	this->board_hw.init();
+	
+	this->counter.init(0xFFFF, 900);
+	
+	this->out_voltage = 0;
+	this->out_current = 0;
+	this->battery_voltage = 12000;
+	this->battery_current = 0;
+	
+	this->btn_up.init(&(this->board_hw.din4), 0, &(this->counter), 10);
+	this->btn_up.update_din = 0;
+	
+	this->btn_down.init(&(this->board_hw.din3), 0, &(this->counter), 10);
+	this->btn_down.update_din = 0;
+	
+	this->btn_mode.init(&(this->board_hw.din1), 0, &(this->counter), 10);
+	this->btn_mode.update_din = 0;
+	
+	this->handbrake.init(&(this->board_hw.hvdin3), 0, &(this->counter), 10);
+	this->handbrake.update_din = 0;
+	
+	this->brakes.init(&(this->board_hw.hvdin2), 1, &(this->counter), 10);
+	this->brakes.update_din = 0;
+	
+	this->dimm.init(&(this->board_hw.hvdin1), 1, &(this->counter), 10);
+	this->dimm.update_din = 0;
+	
+	this->pot.init(&(this->board_hw.ain2), 500, 4500);
+	this->pot.update_ain = 0;
+	
+	this->outdriver.init(&(this->board_hw.out_pwm), &(this->board_hw.out_low));
+	
+	LedDisplay::doutCfg_t dsp_cfg;
+	dsp_cfg.led0_dout_ch = &(this->board_hw.od1);
+	dsp_cfg.led1_dout_ch = &(this->board_hw.od2);
+	dsp_cfg.led2_dout_ch = &(this->board_hw.od3);
+	dsp_cfg.led3_dout_ch = &(this->board_hw.od4);
+	dsp_cfg.led4_dout_ch = &(this->board_hw.od5);
+	dsp_cfg.led5_dout_ch = &(this->board_hw.od6);
+	
+	this->display.init(&dsp_cfg, 0, &(this->counter), &(this->board_hw.od_pwm));
+	
+	// Apply configuration
+	if(cfg->handbrake_pull_up)
+	{
+		this->board_hw.hvdin3_pull.write(1);
+	}
+	else this->board_hw.hvdin3_pull.write(0);
+	
+	if(cfg->speed_hall)
+	{
+		this->board_hw.freq_pull.write(1);
+	}
+	else this->board_hw.freq_pull.write(0);
+	
+	// Set initial output states
+	this->outdriver.write((uint16_t)0);
+	this->outdriver.enable();
+	
+	this->display.write_backlight(100);
+	this->display.write(0x00);
+}
+
+void hw::DccdHw::read(void)
+{
+	// Update low level inputs
+	this->board_hw.read();
+	
+	this->counter.increment();
+	
+	this->out_voltage = this->board_hw.out_voltage.last_read;
+	this->out_current = this->board_hw.out_current.last_read;
+	this->battery_voltage = this->board_hw.battery_voltage.last_read;
+	this->battery_current = this->board_hw.battery_current.last_read;
+	
+	this->btn_up.process();
+	this->btn_down.process();
+	this->btn_mode.process();
+	this->handbrake.process();
+	this->brakes.process();
+	this->dimm.process();
+	
+	this->pot.read();
+}
+
+/**** Private function definitions ***/
diff --git a/firmware/src/hw/dccd_hw.h b/firmware/src/hw/dccd_hw.h
new file mode 100644
index 0000000..2a96053
--- /dev/null
+++ b/firmware/src/hw/dccd_hw.h
@@ -0,0 +1,64 @@
+#ifndef DCCD_HW_H_
+#define DCCD_HW_H_
+
+/**** Includes ****/
+#include <stdint.h>
+#include "../bsp/board.h"
+#include "../utils/vcounter.h"
+#include "button.h"
+#include "led_display.h"
+#include "potentiometer.h"
+#include "out_driver.h"
+
+namespace hw {
+
+/**** Public definitions ****/
+class DccdHw
+{
+	public:
+	typedef struct {
+		uint8_t handbrake_pull_up;
+		uint8_t speed_hall;
+	} dccdHwCfg_t;
+		
+	DccdHw(void);
+	~DccdHw(void);
+	
+	void init(dccdHwCfg_t* cfg);
+	
+	// Inputs
+	uint16_t out_voltage;
+	uint16_t out_current;
+	uint16_t battery_voltage;
+	uint16_t battery_current;
+	
+	Button btn_up;
+	Button btn_down;
+	Button btn_mode;
+	Button handbrake;
+	Button brakes;
+	Button dimm;
+	
+	Potentiometer pot;
+	
+	// Outputs
+	LedDisplay display;
+	OutDriver outdriver;
+	
+	void read(void);
+	
+	#ifdef TESTING
+	protected:
+	#endif
+	bsp::Board board_hw;
+	util::VCounter counter;
+};
+
+/**** Public function declarations ****/
+
+#ifdef TESTING
+#endif
+
+} //namespace
+
+#endif /* DCCD_HW_H_ */
\ No newline at end of file
diff --git a/firmware/src/hw/devices.cpp b/firmware/src/hw/devices.cpp
deleted file mode 100644
index a1a105b..0000000
--- a/firmware/src/hw/devices.cpp
+++ /dev/null
@@ -1,67 +0,0 @@
-/**** Includes ****/
-#include "../utils/utils.h"
-#include "devices.h"
-
-/**** Private definitions ****/
-/**** Private constants ****/
-static const uint16_t def_button_hold_time = 1000;
-static const uint16_t def_max_voltage = 7000;
-static const uint16_t def_min_voltage = 100;
-static const uint16_t def_fuse_treshold = 6000;
-static const uint16_t def_fuse_hold_cycles = 50;
-static const uint16_t def_fuse_cooldown_cycles = 1000;
-
-/**** Private variables ****/
-/**** Private function declarations ****/
-/**** Public function definitions ****/
-void devices_init(void)
-{
-	board_init();
-	
-	btn_up.hold_time = def_button_hold_time;
-	btn_down.hold_time = def_button_hold_time;
-	
-	ccout.max_voltage = def_max_voltage;
-	ccout.max_current = 0;
-	ccout.min_voltage = def_min_voltage;
-	
-	sup_fuse.hold_current = def_fuse_treshold;
-	sup_fuse.trip_cycles = def_fuse_hold_cycles;
-	sup_fuse.cooldown_cycles = def_fuse_cooldown_cycles;
-	
-	out_fuse.hold_current = def_fuse_treshold;
-	out_fuse.trip_cycles = def_fuse_hold_cycles;
-	out_fuse.cooldown_cycles = def_fuse_cooldown_cycles;
-	
-	hvdin3_pull.write(bsp::DOUT_HIGH);
-	
-	devices_update_inputs();
-	
-	display.write(0x00);
-	display.set_brigthness(100);
-	
-	ccout.update();
-	ccout.enable();
-}
-
-void devices_update_inputs(void)
-{
-	board_read();
-	
-	pot.update();
-	
-	btn_mode.update();
-	btn_up.update();
-	btn_down.update();
-	
-	sw_dimm.update();
-	sw_brakes.update();
-	sw_hbrake.update();
-	
-	sup_fuse.update();
-	out_fuse.update();
-	
-	display.process_timer();
-}
-
-/**** Private function definitions ****/
diff --git a/firmware/src/hw/devices.h b/firmware/src/hw/devices.h
deleted file mode 100644
index 01395f3..0000000
--- a/firmware/src/hw/devices.h
+++ /dev/null
@@ -1,34 +0,0 @@
-#ifndef HW_DEVICES_H_
-#define HW_DEVICES_H_
-
-/**** Includes ****/
-#include <stdint.h>
-
-#include "../bsp/board.h"
-
-#include "button.h"
-#include "potentiometer.h"
-#include "display_led.h"
-#include "reg_out.h"
-#include "fuse.h"
-
-static hw::Button btn_mode = hw::Button(&din1, bsp::DIN_LOW, 10, hw::BUTTON_OFF);
-static hw::Button btn_up = hw::Button(&din4, bsp::DIN_LOW, 10, hw::BUTTON_OFF);
-static hw::Button btn_down = hw::Button(&din3, bsp::DIN_LOW, 10, hw::BUTTON_OFF);
-
-static hw::Button sw_dimm = hw::Button(&hvdin1, bsp::DIN_HIGH, 10, hw::BUTTON_OFF);
-static hw::Button sw_brakes = hw::Button(&hvdin2, bsp::DIN_HIGH, 10, hw::BUTTON_OFF);
-static hw::Button sw_hbrake = hw::Button(&hvdin3, bsp::DIN_LOW, 10, hw::BUTTON_OFF);
-
-static hw::Potentiometer pot = hw::Potentiometer(&ain2, 500, 4500);
-
-static hw::DisplayLed display = hw::DisplayLed(&odout1, &odout2, &odout3, &odout4, &odout5, &odout6, &od_pwm);
-
-static hw::RegOut ccout = hw::RegOut(&hbridge, &bat_u, &dccd_u, &dccd_i);
-static hw::Fuse sup_fuse =  hw::Fuse(&bat_i);
-static hw::Fuse out_fuse = hw::Fuse(&dccd_i);
-
-void devices_init(void);
-void devices_update_inputs(void);
-
-#endif /* BSP_BOARD_H_ */
\ No newline at end of file
diff --git a/firmware/src/hw/display_led.cpp b/firmware/src/hw/display_led.cpp
deleted file mode 100644
index af9085b..0000000
--- a/firmware/src/hw/display_led.cpp
+++ /dev/null
@@ -1,195 +0,0 @@
-/**** Includes ****/
-#include "../utils/utils.h"
-#include "display_led.h"
-
-using namespace hw;
-
-/**** Private definitions ****/
-/**** Private constants ****/
-/**** Private variables ****/
-/**** Private function declarations ****/
-static uint8_t img_gen_dot10(uint8_t percent);
-static uint8_t img_gen_dot20(uint8_t percent);
-static uint8_t img_gen_bar(uint8_t percent);
-
-/**** Public function definitions ****/
-hw::DisplayLed::DisplayLed(bsp::DigitalOut* led0, bsp::DigitalOut* led1, bsp::DigitalOut* led2, bsp::DigitalOut* led3, bsp::DigitalOut* led4, bsp::DigitalOut* led5, bsp::PWMout* common)
-{
-	this->led0 = led0;
-	this->led1 = led1;
-	this->led2 = led2;
-	this->led3 = led3;
-	this->led4 = led4;
-	this->led5 = led5;
-	this->common = common;
-	
-	this->led0->write(0);
-	this->led1->write(0);
-	this->led2->write(0);
-	this->led3->write(0);
-	this->led4->write(0);
-	this->led5->write(0);
-	this->common->write(0);
-	
-	this->lock_counter = 0;
-	this->locked = 1;
-}
-
-hw::DisplayLed::~DisplayLed(void)
-{
-	this->led0->write(0);
-	this->led1->write(0);
-	this->led2->write(0);
-	this->led3->write(0);
-	this->led4->write(0);
-	this->led5->write(0);
-	this->common->write(0);
-}
-
-void hw::DisplayLed::show_percent(uint8_t percent, style_t style)
-{
-	uint8_t image = 0x00;
-	
-	switch(style)
-	{
-		case LED_DSP_BAR:
-		image = img_gen_bar(percent);
-		break;
-		
-		case LED_DSP_DOT10:
-		image = img_gen_dot10(percent);
-		break;
-		
-		default:
-		image = img_gen_dot20(percent);
-		break;
-	}
-	
-	this->write(image);
-}
-
-void hw::DisplayLed::write(uint8_t image)
-{
-	if(image&0x01) this->led0->write(1);
-	else this->led0->write(0);
-	
-	if(image&0x02) this->led1->write(1);
-	else this->led1->write(0);
-	
-	if(image&0x04) this->led2->write(1);
-	else this->led2->write(0);
-	
-	if(image&0x08) this->led3->write(1);
-	else this->led3->write(0);
-	
-	if(image&0x10) this->led4->write(1);
-	else this->led4->write(0);
-	
-	if(image&0x20) this->led5->write(1);
-	else this->led5->write(0);
-}
-
-void hw::DisplayLed::set_brigthness(uint8_t percent)
-{
-	this->common->write(percent);
-}
-
-void hw::DisplayLed::set_lock(uint16_t timeout)
-{
-	this->lock_counter = timeout;
-	this->locked = 1;
-}
-
-void hw::DisplayLed::process_timer(void)
-{
-	if(this->lock_counter) this->lock_counter--;
-	else this->locked = 0;
-}
-
-/**** Private function definitions ****/
-static uint8_t img_gen_dot10(uint8_t percent)
-{
-	switch(percent)
-	{
-		case 0 ... 5:
-		return 0x01;
-		
-		case 6 ... 15:
-		return 0x03;
-		
-		case 16 ... 25:
-		return 0x02;
-		
-		case 26 ... 35:
-		return 0x06;
-		
-		case 36 ... 45:
-		return 0x04;
-		
-		case 46 ... 55:
-		return 0x0C;
-		
-		case 56 ... 65:
-		return 0x08;
-		
-		case 66 ... 75:
-		return 0x18;
-		
-		case 76 ... 85:
-		return 0x10;
-		
-		case 86 ... 95:
-		return 0x30;
-		
-		default:
-		return 0x20;
-	}
-}
-
-static uint8_t img_gen_dot20(uint8_t percent)
-{
-	switch(percent)
-	{
-		case 0 ... 10:
-		return 0x01;
-		
-		case 11 ... 30:
-		return 0x02;
-		
-		case 31 ... 50:
-		return 0x04;
-		
-		case 51 ... 70:
-		return 0x08;
-		
-		case 71 ... 90:
-		return 0x10;
-		
-		default:
-		return 0x20;
-	}
-}
-
-static uint8_t img_gen_bar(uint8_t percent)
-{
-	switch(percent)
-	{
-		case 0 ... 10:
-		return 0x01;
-		
-		case 11 ... 30:
-		return 0x03;
-		
-		case 31 ... 50:
-		return 0x07;
-		
-		case 51 ... 70:
-		return 0x0F;
-		
-		case 71 ... 90:
-		return 0x1F;
-		
-		default:
-		return 0x3F;
-	}
-}
\ No newline at end of file
diff --git a/firmware/src/hw/display_led.h b/firmware/src/hw/display_led.h
deleted file mode 100644
index 6e73567..0000000
--- a/firmware/src/hw/display_led.h
+++ /dev/null
@@ -1,54 +0,0 @@
-#ifndef DISPLAY_LED_H_
-#define DISPLAY_LED_H_
-
-/**** Includes ****/
-#include <stdint.h>
-#include "../bsp/dout.h"
-#include "../bsp/pwm.h"
-
-namespace hw {
-
-/**** Public definitions ****/
-
-class DisplayLed
-{
-	protected:
-	bsp::DigitalOut* led0;
-	bsp::DigitalOut* led1;
-	bsp::DigitalOut* led2;
-	bsp::DigitalOut* led3;
-	bsp::DigitalOut* led4;
-	bsp::DigitalOut* led5;
-	bsp::PWMout* common;
-	
-	uint16_t lock_counter;
-	
-	public:
-	typedef enum {
-		LED_DSP_DOT20,
-		LED_DSP_DOT10,
-		LED_DSP_BAR
-	} style_t;
-	
-	DisplayLed(bsp::DigitalOut* led0, bsp::DigitalOut* led1, bsp::DigitalOut* led2, bsp::DigitalOut* led3, bsp::DigitalOut* led4, bsp::DigitalOut* led5, bsp::PWMout* common);
-	~DisplayLed(void);
-	
-	void show_percent(uint8_t percent, style_t style);
-	void write(uint8_t image);
-	
-	void set_brigthness(uint8_t percent);
-	
-	void set_lock(uint16_t timeout);
-	void process_timer(void);
-	
-	uint8_t locked;
-};
-
-/**** Public function declarations ****/
-
-#ifdef TESTING
-#endif
-
-} //namespace
-
-#endif /* DISPLAY_LED_H_ */
\ No newline at end of file
diff --git a/firmware/src/hw/fuse.cpp b/firmware/src/hw/fuse.cpp
deleted file mode 100644
index 51d8cae..0000000
--- a/firmware/src/hw/fuse.cpp
+++ /dev/null
@@ -1,66 +0,0 @@
-/**** Includes ****/
-#include "../utils/utils.h"
-#include "fuse.h"
-
-using namespace hw;
-
-/**** Private definitions ****/
-/**** Private constants ****/
-/**** Private variables ****/
-/**** Private function declarations ****/
-
-/**** Public function definitions ****/
-hw::Fuse::Fuse(bsp::AnalogIn* ain_ch)
-{
-	this->hold_current = 0;
-	this->trip_cycles = 0;
-	this->warning = 0;
-	this->fault = 0;
-	this->cooldown_counter = 0;
-	this->cooldown_cycles = 0;
-	this->retry_cnt = 0;
-}
-
-hw::Fuse::~Fuse(void)
-{
-	return;
-}
-
-void hw::Fuse::update(void)
-{
-	// Under threshold
-	if(this->ain_ch->last_read <= this->hold_current)
-	{
-		// Clear warning flag
-		this->warning = 0;
-		
-		// OC energy counter
-		if(this->oc_counter > 0) this->oc_counter--;
-		
-		// Cool down fuse
-		if(this->cooldown_counter > 0) this->cooldown_counter--;
-		
-		// Auto reset logic
-		if((this->fault)&&(this->cooldown_counter==0))
-		{
-			this->fault = 0;
-			this->retry_cnt = util::sat_add(this->retry_cnt, 1);
-		};
-		return;
-	};
-
-	// Over current condition
-	this->warning = 1;
-		
-	// PC energy counter
-	this->oc_counter = util::sat_add(this->oc_counter, 1);
-		
-	// Check for trip threshold
-	if(this->oc_counter < this->trip_cycles) return;
-		
-	// Trip fuse
-	this->fault = 1;
-	this->cooldown_counter = this->cooldown_cycles;
-}
-
-/**** Private function definitions ****/
diff --git a/firmware/src/hw/fuse.h b/firmware/src/hw/fuse.h
deleted file mode 100644
index ae789c8..0000000
--- a/firmware/src/hw/fuse.h
+++ /dev/null
@@ -1,40 +0,0 @@
-#ifndef FUSE_H_
-#define FUSE_H_
-
-/**** Includes ****/
-#include <stdint.h>
-#include "../bsp/ain.h"
-
-namespace hw {
-
-/**** Public definitions ****/
-
-class Fuse
-{
-	protected:
-	bsp::AnalogIn* ain_ch;
-	uint16_t oc_counter;
-	uint16_t cooldown_counter;
-	
-	public:
-	Fuse(bsp::AnalogIn* ain_ch);
-	~Fuse(void);
-	
-	uint16_t hold_current;
-	uint16_t trip_cycles;
-	uint8_t warning;
-	uint8_t fault;
-	uint16_t cooldown_cycles;
-	uint8_t retry_cnt;
-	
-	void update(void);
-};
-
-/**** Public function declarations ****/
-
-#ifdef TESTING
-#endif
-
-} //namespace
-
-#endif /* POTENTIOMETER_H_ */
\ No newline at end of file
diff --git a/firmware/src/hw/led_display.cpp b/firmware/src/hw/led_display.cpp
new file mode 100644
index 0000000..867e7f0
--- /dev/null
+++ b/firmware/src/hw/led_display.cpp
@@ -0,0 +1,157 @@
+/**** Includes ****/
+#include "../utils/utils.h"
+#include "led_display.h"
+
+using namespace hw;
+
+/**** Private definitions ****/
+/**** Private constants ****/
+/**** Private variables ****/
+/**** Private function declarations ****/
+/**** Public function definitions ****/
+hw::LedDisplay::LedDisplay(void)
+{
+	return;
+}
+
+hw::LedDisplay::~LedDisplay(void)
+{
+	this->force(0x00);
+	this->write_backlight(0);
+}
+
+void hw::LedDisplay::init(doutCfg_t* dout_chs, uint8_t act_lvl, util::VCounter* timer, bsp::PwmOut* pwm_ch)
+{
+	this->led0_dout_ch = dout_chs->led0_dout_ch;
+	this->led1_dout_ch = dout_chs->led1_dout_ch;
+	this->led2_dout_ch = dout_chs->led2_dout_ch;
+	this->led3_dout_ch = dout_chs->led3_dout_ch;
+	this->led4_dout_ch = dout_chs->led4_dout_ch;
+	this->led5_dout_ch = dout_chs->led5_dout_ch;
+	
+	if(act_lvl) this->act_lvl = 1;
+	else this->act_lvl = 0;
+	
+	this->timer = timer;
+	
+	this->pwm_ch = pwm_ch;
+	
+	this->on_time = 0;
+	this->period = 0;
+	this->cycle_cnt = 0;
+	this->cycle_limit = 0;
+	this->timestamp_start = 0;
+	
+	this->force(0x00);
+	this->write_backlight(0);
+}
+
+void hw::LedDisplay::force(uint8_t image)
+{
+	uint8_t led_state;
+	
+	if(image&0x01) led_state = 1;
+	else led_state = 0;
+	this->set_single_led(led_state, this->led0_dout_ch);
+	
+	if(image&0x02) led_state = 1;
+	else led_state = 0;
+	this->set_single_led(led_state, this->led1_dout_ch);
+	
+	if(image&0x04) led_state = 1;
+	else led_state = 0;
+	this->set_single_led(led_state, this->led2_dout_ch);
+	
+	if(image&0x08) led_state = 1;
+	else led_state = 0;
+	this->set_single_led(led_state, this->led3_dout_ch);
+	
+	if(image&0x10) led_state = 1;
+	else led_state = 0;
+	this->set_single_led(led_state, this->led4_dout_ch);
+	
+	if(image&0x20) led_state = 1;
+	else led_state = 0;
+	this->set_single_led(led_state, this->led5_dout_ch);
+}
+
+void hw::LedDisplay::write(uint8_t image)
+{
+	// Static mode
+	this->on_time = 1;
+	this->period = 0;
+	this->cycle_cnt = 0;
+	this->cycle_limit = 0;
+	this->timestamp_start = 0;
+	
+	// Set initial state
+	this->force(image);
+}
+
+void hw::LedDisplay::write(uint8_t image, uint16_t on_time, uint16_t period, uint8_t cycle_limit)
+{
+	// "PWM" mode
+	this->on_time = on_time;
+	this->period = period;
+	this->cycle_cnt = 0;
+	this->cycle_limit = cycle_limit;
+	
+	// Set initial state
+	if(this->on_time > 0) this->force(image);
+	else this->force(0x00);
+	
+	// Cycle start time
+	this->timestamp_start = this->timer->read();
+}
+
+void hw::LedDisplay::process(void)
+{
+	if(this->period == 0) return; // Nothing to do
+	
+	// Update cycle timing
+	uint16_t ts_now = this->timer->read();
+	uint16_t td = util::time_delta(this->timestamp_start, ts_now);
+	uint32_t td_ms = this->timer->convert_ms(td);
+	
+	if(td_ms >= this->period)
+	{
+		this->timestamp_start = ts_now;
+		this->cycle_cnt++;
+	};
+	
+	// Check cycle limit
+	if((this->cycle_cnt >= this->cycle_limit)&&(this->cycle_limit))
+	{
+		this->on_time = 0;
+		this->period = 0;
+		this->timestamp_start = 0;
+		this->force(0x00);
+		return;
+	};
+	
+	// Do output compare
+	if(td_ms < this->on_time) this->force(this->image);
+	else this->force(0x00);
+}
+
+uint8_t hw::LedDisplay::is_cycle_end(void)
+{
+	if(this->cycle_cnt >= this->cycle_limit) return 1;
+	else return 0; 
+}
+
+void hw::LedDisplay::write_backlight(uint8_t percent)
+{
+	this->pwm_ch->write(percent);
+}
+
+void hw::LedDisplay::set_single_led(uint8_t state, bsp::DigitalOut* led_ch)
+{
+	uint8_t lvl = 0;
+	
+	if(((state==0)&&(this->act_lvl==0))||((state!=0)&&(this->act_lvl==1))) lvl = 1;
+	
+	led_ch->write(lvl);
+}
+
+/**** Private function definitions ***/
diff --git a/firmware/src/hw/led_display.h b/firmware/src/hw/led_display.h
new file mode 100644
index 0000000..564d163
--- /dev/null
+++ b/firmware/src/hw/led_display.h
@@ -0,0 +1,66 @@
+#ifndef LED_DISPLAY_H_
+#define LED_DISPLAY_H_
+
+/**** Includes ****/
+#include <stdint.h>
+#include "../utils/vcounter.h"
+#include "../bsp/board.h"
+
+namespace hw {
+
+/**** Public definitions ****/
+class LedDisplay
+{
+	public:
+	typedef struct {
+		bsp::DigitalOut* led0_dout_ch;
+		bsp::DigitalOut* led1_dout_ch;
+		bsp::DigitalOut* led2_dout_ch;
+		bsp::DigitalOut* led3_dout_ch;
+		bsp::DigitalOut* led4_dout_ch;
+		bsp::DigitalOut* led5_dout_ch;
+	} doutCfg_t;
+	
+	LedDisplay(void);
+	~LedDisplay(void);
+	
+	uint16_t on_time;
+	uint16_t period;
+	uint8_t cycle_cnt;
+	uint8_t cycle_limit;
+	
+	void init(doutCfg_t* dout_chs, uint8_t act_lvl, util::VCounter* timer, bsp::PwmOut* pwm_ch);
+	void write(uint8_t image);
+	void write(uint8_t image, uint16_t on_time, uint16_t period, uint8_t cycle_limit);
+	void process(void);
+	uint8_t is_cycle_end(void);
+	void force(uint8_t image);
+	
+	void write_backlight(uint8_t percent);
+	
+	#ifdef TESTING
+	protected:
+	#endif
+	bsp::DigitalOut* led0_dout_ch;
+	bsp::DigitalOut* led1_dout_ch;
+	bsp::DigitalOut* led2_dout_ch;
+	bsp::DigitalOut* led3_dout_ch;
+	bsp::DigitalOut* led4_dout_ch;
+	bsp::DigitalOut* led5_dout_ch;
+	uint8_t act_lvl;
+	util::VCounter* timer;
+	bsp::PwmOut* pwm_ch;
+	uint16_t timestamp_start;
+	uint8_t image;
+	
+	void set_single_led(uint8_t state, bsp::DigitalOut* led_ch);
+};
+
+/**** Public function declarations ****/
+
+#ifdef TESTING
+#endif
+
+} //namespace
+
+#endif /* LED_DISPLAY_H_ */
\ No newline at end of file
diff --git a/firmware/src/hw/out_driver.cpp b/firmware/src/hw/out_driver.cpp
new file mode 100644
index 0000000..6701ce6
--- /dev/null
+++ b/firmware/src/hw/out_driver.cpp
@@ -0,0 +1,105 @@
+/**** Includes ****/
+#include "../utils/utils.h"
+#include "out_driver.h"
+
+using namespace hw;
+
+/**** Private definitions ****/
+/**** Private constants ****/
+/**** Private variables ****/
+/**** Private function declarations ****/
+
+/**** Public function definitions ****/
+hw::OutDriver::OutDriver(void)
+{
+	return;
+}
+
+hw::OutDriver::~OutDriver(void)
+{
+	return;
+}
+
+void hw::OutDriver::init(bsp::PwmOut* pwm_high, bsp::DigitalOut* dout_low)
+{
+	this->pwm_high = pwm_high;
+	this->dout_low = dout_low;
+	
+	this->target_duty = 0;
+	this->target_low = 0;
+	this->disabled = 1;
+}
+
+void hw::OutDriver::write(uint16_t numerator)
+{	
+	this->target_duty = numerator;
+	this->target_low = 1;
+	
+	// Check if enabled
+	if(this->disabled)
+	{
+		return;
+	};
+	
+	// Set low side
+	if(this->dout_low->last_writen == 0)
+	{
+		this->dout_low->write(this->target_low);
+	};
+	
+	// Set PWM
+	this->pwm_high->write(this->target_duty);
+}
+
+void hw::OutDriver::write(uint8_t percent)
+{
+	// Convert to numerator/0xFFFF
+	this->write(util::percent_to_16b(percent));
+}
+
+void hw::OutDriver::write_hiz(void)
+{
+	this->target_duty = 0;
+	this->target_low = 0;
+	
+	// Check if enabled
+	if(this->disabled)
+	{
+		return;
+	};
+	
+	// Set PWM
+	this->pwm_high->write((uint16_t)0);
+	
+	// Set low side
+	this->dout_low->write(0);
+}
+
+void hw::OutDriver::enable(void)
+{
+	if(this->disabled==0) return;
+	this->disabled = 0;
+	
+	if(this->target_low==0) this->write_hiz();
+	else this->write(this->target_duty);
+}
+
+void hw::OutDriver::disable(void)
+{
+	if(this->disabled!=0) return;
+	
+	// Set PWM
+	this->pwm_high->write((uint16_t)0);
+	
+	// Set low side
+	this->dout_low->write(0);
+	
+	this->disabled = 1;
+}
+
+uint8_t hw::OutDriver::is_disabled(void)
+{
+	return this->disabled;
+}
+
+/**** Private function definitions ****/
diff --git a/firmware/src/hw/out_driver.h b/firmware/src/hw/out_driver.h
new file mode 100644
index 0000000..b6418e9
--- /dev/null
+++ b/firmware/src/hw/out_driver.h
@@ -0,0 +1,45 @@
+#ifndef OUT_DRIVER_H_
+#define OUT_DRIVER_H_
+
+/**** Includes ****/
+#include <stdint.h>
+#include "../bsp/board.h"
+
+namespace hw {
+
+/**** Public definitions ****/
+
+class OutDriver
+{	
+	public:
+	OutDriver(void);
+	~OutDriver(void);
+	
+	void init(bsp::PwmOut* pwm_high, bsp::DigitalOut* dout_low);
+	
+	uint16_t target_duty;
+	uint8_t target_low;
+	
+	void write(uint16_t numerator);
+	void write(uint8_t percent);
+	void write_hiz(void);
+	void enable(void);
+	void disable(void);
+	uint8_t is_disabled(void);
+	
+	#ifndef TESTING
+	protected:
+	#endif
+	bsp::PwmOut* pwm_high;
+	bsp::DigitalOut* dout_low;
+	uint8_t disabled;
+};
+
+/**** Public function declarations ****/
+
+#ifdef TESTING
+#endif
+
+} //namespace
+
+#endif /* OUT_DRIVER_H_ */
\ No newline at end of file
diff --git a/firmware/src/hw/potentiometer.cpp b/firmware/src/hw/potentiometer.cpp
index 617a1b5..2ddbc11 100644
--- a/firmware/src/hw/potentiometer.cpp
+++ b/firmware/src/hw/potentiometer.cpp
@@ -11,12 +11,9 @@ using namespace hw;
 /**** Private function declarations ****/
 
 /**** Public function definitions ****/
-hw::Potentiometer::Potentiometer(bsp::AnalogIn* ain_ch, uint16_t low_deadzone, uint16_t high_deadzone)
+hw::Potentiometer::Potentiometer(void)
 {
-	this->ain_ch = ain_ch;
-	this->low_deadzone = low_deadzone;
-	this->high_deadzone = high_deadzone;
-	this->percent = 0;
+	return;
 }
 
 hw::Potentiometer::~Potentiometer(void)
@@ -24,14 +21,26 @@ hw::Potentiometer::~Potentiometer(void)
 	return;
 }
 
-uint8_t hw::Potentiometer::update(void)
+void hw::Potentiometer::init(bsp::AnalogIn* ain_ch, uint16_t low_deadzone, uint16_t high_deadzone)
 {
-	// Calculate percent
-	if(this->ain_ch->last_read <= this->low_deadzone) this->percent = 0;
-	else if(this->ain_ch->last_read >= this->high_deadzone ) this->percent = 100;
-	else this->percent = util::interpolate(this->ain_ch->last_read, this->low_deadzone, this->high_deadzone, 0, 100);
+	this->ain_ch = ain_ch;
+	this->low_deadzone = low_deadzone;
+	this->high_deadzone = high_deadzone;
+	this->last_percent = 0;
+	this->update_ain = 1;
+}
+
+uint8_t hw::Potentiometer::read(void)
+{
+	// Update analog input
+	if(this->update_ain) this->ain_ch->read();
 	
-	return this->percent;
+	// Calculate percent
+	if(this->ain_ch->last_read <= this->low_deadzone) this->last_percent = 0;
+	else if(this->ain_ch->last_read >= this->high_deadzone ) this->last_percent = 100;
+	else this->last_percent = util::interpolate(this->ain_ch->last_read, this->low_deadzone, this->high_deadzone, 0, 100);
+	
+	return this->last_percent;
 }
 
 /**** Private function definitions ****/
diff --git a/firmware/src/hw/potentiometer.h b/firmware/src/hw/potentiometer.h
index 2321f7f..be65f4d 100644
--- a/firmware/src/hw/potentiometer.h
+++ b/firmware/src/hw/potentiometer.h
@@ -3,26 +3,31 @@
 
 /**** Includes ****/
 #include <stdint.h>
-#include "../bsp/ain.h"
+#include "../bsp/board.h"
 
 namespace hw {
 
 /**** Public definitions ****/
 
 class Potentiometer
-{
-	protected:
-	bsp::AnalogIn* ain_ch;
-	
+{	
 	public:
-	Potentiometer(bsp::AnalogIn* ain_ch, uint16_t low_deadzone, uint16_t high_deadzone);
+	Potentiometer(void);
 	~Potentiometer(void);
 	
+	void init(bsp::AnalogIn* ain_ch, uint16_t low_deadzone, uint16_t high_deadzone);
+	
 	uint16_t low_deadzone;
 	uint16_t high_deadzone;
-	uint8_t percent;
+	uint8_t last_percent;
+	uint8_t update_ain;
 	
-	uint8_t update(void);
+	uint8_t read(void);
+	
+	#ifndef TESTING
+	protected:
+	#endif
+	bsp::AnalogIn* ain_ch;
 };
 
 /**** Public function declarations ****/
diff --git a/firmware/src/hw/reg_out.cpp b/firmware/src/hw/reg_out.cpp
deleted file mode 100644
index 9edc744..0000000
--- a/firmware/src/hw/reg_out.cpp
+++ /dev/null
@@ -1,71 +0,0 @@
-/**** Includes ****/
-#include "../utils/utils.h"
-#include "reg_out.h"
-
-using namespace hw;
-
-/**** Private definitions ****/
-/**** Private constants ****/
-/**** Private variables ****/
-/**** Private function declarations ****/
-
-/**** Public function definitions ****/
-hw::RegOut::RegOut(bsp::Hafbridge* hbridge, bsp::AnalogIn* sup_u, bsp::AnalogIn* out_u, bsp::AnalogIn* out_i)
-{
-	this->hbridge = hbridge;
-	this->supply_voltage = sup_u;
-	this->out_voltage = out_u;
-	this->out_currnet = out_i;
-	
-	this->max_voltage = 6750;
-	this->max_current = 4500;
-	this->min_voltage = 200;
-	
-	this->set_voltage = 0;
-	this->out_impedance = 0xFFFF;
-	
-	this->hbridge->disable();
-}
-
-hw::RegOut::~RegOut(void)
-{
-	this->hbridge->write((uint16_t)0x0000);
-	this->hbridge->disable();
-	return;
-}
-
-void hw::RegOut::update(void)
-{
-	// Calculate output impedance
-	if((this->out_currnet == 0)||(this->out_voltage->last_read == 0)) this->out_impedance = 0xFFFF;
-	else this->out_impedance = util::sat_div_kilo(this->out_voltage->last_read, this->out_currnet->last_read);
-		
-	// Recalculate output set voltage
-	if((this->max_voltage==0)||(this->max_current==0)) this->set_voltage = 0;
-	else this->set_voltage = util::sat_mul_kilo(this->max_current, this->out_impedance);
-	
-	// Limit set voltage
-	if(this->set_voltage > this->max_voltage) this->set_voltage = this->max_voltage;
-	
-	if((this->set_voltage>0)&&(this->set_voltage < this->min_voltage)) this->set_voltage = this->min_voltage;
-	
-	// Set output
-	this->hbridge->write(util::sat_ratio(this->set_voltage, this->supply_voltage->last_read));
-}
-
-void hw::RegOut::enable(void)
-{
-	this->hbridge->enable();
-}
-
-void hw::RegOut::disable(void)
-{
-	this->hbridge->disable();	
-}
-
-uint8_t hw::RegOut::is_enabled(void)
-{
-	return this->hbridge->is_enabled();
-}
-
-/**** Private function definitions ****/
diff --git a/firmware/src/hw/reg_out.h b/firmware/src/hw/reg_out.h
deleted file mode 100644
index f2356c7..0000000
--- a/firmware/src/hw/reg_out.h
+++ /dev/null
@@ -1,45 +0,0 @@
-#ifndef REGULATED_OUTPUT_H_
-#define REGULATED_OUTPUT_H_
-
-/**** Includes ****/
-#include <stdint.h>
-#include "../bsp/ain.h"
-#include "../bsp/halfbridge.h"
-
-namespace hw {
-
-/**** Public definitions ****/
-
-class RegOut
-{
-	protected:
-	bsp::Hafbridge* hbridge;
-	bsp::AnalogIn* supply_voltage;
-	bsp::AnalogIn* out_voltage;
-	bsp::AnalogIn* out_currnet;
-	
-	public:
-	RegOut(bsp::Hafbridge* hbridge, bsp::AnalogIn* sup_u, bsp::AnalogIn* out_u, bsp::AnalogIn* out_i);
-	~RegOut(void);
-	
-	uint16_t max_voltage;
-	uint16_t max_current;
-	uint16_t min_voltage;
-	
-	uint16_t set_voltage;	
-	uint16_t out_impedance;
-	
-	void update(void);
-	void enable(void);
-	void disable(void);
-	uint8_t is_enabled(void);
-};
-
-/**** Public function declarations ****/
-
-#ifdef TESTING
-#endif
-
-} //namespace
-
-#endif /* REGULATED_OUTPUT_H_ */
\ No newline at end of file
diff --git a/firmware/src/logic/button_force.cpp b/firmware/src/logic/button_force.cpp
deleted file mode 100644
index 59f70ad..0000000
--- a/firmware/src/logic/button_force.cpp
+++ /dev/null
@@ -1,51 +0,0 @@
-/**** Includes ****/
-#include "../utils/utils.h"
-#include "button_force.h"
-
-using namespace logic;
-
-/**** Private definitions ****/
-/**** Private constants ****/
-/**** Private variables ****/
-/**** Private function declarations ****/
-
-/**** Public function definitions ****/
-logic::ButtonForce::ButtonForce(hw::Button* btn_up, hw::Button* btn_down)
-{
-	this->btn_up = btn_up;
-	this->btn_down = btn_down;
-	this->force = 0;
-	this->step = 10;
-	this->is_new = 0;
-}
-
-logic::ButtonForce::~ButtonForce(void)
-{
-	return;
-}
-
-uint8_t logic::ButtonForce::update(void)
-{	
-	uint8_t next_force = 0;
-	
-	if((this->btn_up->is_new)&&(this->btn_up->state == hw::BUTTON_ON))
-	{
-		next_force = util::sat_add(this->force, this->step);
-		if(next_force > 100) next_force = 100;
-		this->btn_up->is_new = 0;
-	};
-	
-	if((this->btn_down->is_new)&&(this->btn_down->state == hw::BUTTON_ON))
-	{
-		next_force = util::sat_subtract(this->force, this->step);
-		this->btn_down->is_new = 0;
-	};
-	
-	if(next_force != this->force) this->is_new = 1;
-	
-	this->force = next_force;
-	
-	return this->force;
-}
-
-/**** Private function definitions ****/
diff --git a/firmware/src/logic/button_force.h b/firmware/src/logic/button_force.h
deleted file mode 100644
index 4dd81e7..0000000
--- a/firmware/src/logic/button_force.h
+++ /dev/null
@@ -1,36 +0,0 @@
-#ifndef BUTTON_FORCE_H_
-#define BUTTON_FORCE_H_
-
-/**** Includes ****/
-#include <stdint.h>
-#include "../hw/button.h"
-
-namespace logic {
-
-/**** Public definitions ****/
-
-class ButtonForce
-{
-	protected:
-	hw::Button* btn_up;
-	hw::Button* btn_down;
-	
-	public:
-	ButtonForce(hw::Button* btn_up, hw::Button* btn_down);
-	~ButtonForce(void);
-
-	uint8_t force;
-	uint8_t step;
-	uint8_t is_new;
-	
-	uint8_t update(void);
-};
-
-/**** Public function declarations ****/
-
-#ifdef TESTING
-#endif
-
-} //namespace
-
-#endif /* BUTTON_FORCE_H_ */
\ No newline at end of file
diff --git a/firmware/src/logic/cfg_mem.cpp b/firmware/src/logic/cfg_mem.cpp
deleted file mode 100644
index eea5229..0000000
--- a/firmware/src/logic/cfg_mem.cpp
+++ /dev/null
@@ -1,193 +0,0 @@
-/**** Includes ****/
-#include "cfg_mem.h"
-#include "../bsp/mcu/mcu_hal.h"
-
-using namespace logic;
-
-/**** Private definitions ****/
-/**** Private constants ****/
-static const uint16_t addr_btn_force       = 0x0000;
-static const uint16_t addr_bmode           = 0x0001;
-static const uint16_t addr_pot_mode        = 0x0002;
-static const uint16_t addr_dsp_brigth      = 0x0003;
-static const uint16_t addr_dsp_dimm        = 0x0004;
-static const uint16_t addr_brake_force     = 0x0005;
-static const uint16_t addr_max_hbrake_time = 0x0006;
-static const uint16_t addr_lock_current    = 0x0008;
-static const uint16_t addr_max_out_voltage = 0x000A;
-static const uint16_t addr_min_out_voltage = 0x000C;
-static const uint16_t addr_cfg_good        = 0x000D;
-
-static const uint8_t def_btn_force = 0;
-static const uint8_t def_pot_mode = 0;
-static const uint8_t def_bmode = 0;
-static const uint8_t def_dimm = 50;
-static const uint8_t def_brigth = 100;
-static const uint8_t def_brake_force = 100;
-static const uint16_t def_max_hbrake_time = 1000;
-static const uint16_t def_lock_current = 4500;
-static const uint16_t def_max_out_voltage = 7000;
-static const uint16_t def_min_out_voltage = 200;
-
-/**** Private variables ****/
-/**** Private function declarations ****/
-/**** Public function definitions ****/
-logic::CfgMemory::CfgMemory(void)
-{	
-	this->cfg_good    = 0;
-	
-	this->mem_btn_force    = 0;
-	this->mem_bmode        = 0;
-	this->mem_pot_mode     = 0;
-	this->mem_dsp_brigth   = 0;
-	this->mem_dsp_dimm     = 0;
-	this->mem_brake_force     = 0;
-	this->mem_max_hbrake_time = 0;
-	this->mem_lock_current    = 0;
-	this->mem_max_out_voltage = 0;
-	this->mem_min_out_voltage = 0;
-	
-	this->restore();
-}
-
-logic::CfgMemory::~CfgMemory(void)
-{
-	return;
-}
-
-void logic::CfgMemory::init(void)
-{
-	uint8_t cfg_good_magic    = mcu::eeprom_read8b(addr_cfg_good);
-	this->mem_btn_force       = mcu::eeprom_read8b(addr_btn_force);
-	this->mem_bmode           = mcu::eeprom_read8b(addr_bmode);
-	this->mem_pot_mode        = mcu::eeprom_read8b(addr_pot_mode);
-	this->mem_dsp_brigth      = mcu::eeprom_read8b(addr_dsp_brigth);
-	this->mem_dsp_dimm        = mcu::eeprom_read8b(addr_dsp_dimm);
-	this->mem_brake_force     = mcu::eeprom_read8b(addr_brake_force);
-	this->mem_max_hbrake_time = mcu::eeprom_read16b(addr_max_hbrake_time);
-	this->mem_lock_current    = mcu::eeprom_read16b(addr_lock_current);
-	this->mem_max_out_voltage = mcu::eeprom_read16b(addr_max_out_voltage);
-	this->mem_min_out_voltage = mcu::eeprom_read16b(addr_min_out_voltage);
-	
-	// Validate EEPROM data
-	if(cfg_good_magic == 0x37) this->cfg_good = 1;
-	else this->cfg_good = 0;
-	
-	if(this->cfg_good != 1)
-	{
-		this->mem_btn_force = def_btn_force;
-		this->mem_bmode = def_bmode;
-		this->mem_pot_mode = def_pot_mode;
-		this->mem_dsp_brigth = def_brigth;
-		this->mem_dsp_dimm = def_dimm;
-		this->mem_brake_force = def_brake_force;
-		this->mem_max_hbrake_time = def_max_hbrake_time;
-		this->mem_lock_current = def_lock_current;
-		this->mem_max_out_voltage = def_max_out_voltage;
-		this->mem_min_out_voltage = def_min_out_voltage;
-	}
-	
-	this->restore();
-}
-
-void logic::CfgMemory::save(void)
-{
-	if(this->btn_force != this->mem_btn_force)
-	{
-		this->mem_btn_force = this->btn_force;
-		mcu::eeprom_write8b(addr_btn_force, this->mem_btn_force);
-	};	
-	
-	if(this->bmode != this->mem_bmode)
-	{
-		this->mem_bmode = this->bmode;
-		mcu::eeprom_write8b(addr_bmode, this->mem_bmode);
-	};
-}
-
-void logic::CfgMemory::save_all(void)
-{
-	this->save();
-	
-	if(this->pot_mode != this->mem_pot_mode)
-	{
-		this->mem_pot_mode = this->pot_mode;
-		mcu::eeprom_write8b(addr_pot_mode, this->mem_pot_mode);
-	};
-	
-	if(this->dsp_brigth != this->mem_dsp_brigth)
-	{
-		this->mem_dsp_brigth = this->dsp_brigth;
-		mcu::eeprom_write8b(addr_dsp_brigth, this->mem_dsp_brigth);
-	};
-	
-	if(this->dsp_dimm != this->mem_dsp_dimm)
-	{
-		this->mem_dsp_dimm = this->dsp_dimm;
-		mcu::eeprom_write8b(addr_dsp_dimm, this->mem_dsp_dimm);
-	};
-	
-	if(this->brake_force != this->mem_brake_force)
-	{
-		this->mem_brake_force = this->brake_force;
-		mcu::eeprom_write8b(addr_brake_force, this->mem_brake_force);
-	};
-	
-	if(this->max_hbrake_time != this->mem_max_hbrake_time)
-	{
-		this->mem_max_hbrake_time = this->max_hbrake_time;
-		mcu::eeprom_write16b(addr_max_hbrake_time, this->mem_max_hbrake_time);
-	};
-	
-	if(this->lock_current != this->mem_lock_current)
-	{
-		this->mem_lock_current = this->lock_current;
-		mcu::eeprom_write16b(addr_lock_current, this->mem_lock_current);
-	};
-	
-	if(this->max_out_voltage != this->mem_max_out_voltage)
-	{
-		this->mem_max_out_voltage = this->max_out_voltage;
-		mcu::eeprom_write16b(addr_max_out_voltage, this->mem_max_out_voltage);
-	};
-	
-	if(this->min_out_voltage != this->mem_min_out_voltage)
-	{
-		this->mem_min_out_voltage = this->min_out_voltage;
-		mcu::eeprom_write16b(addr_min_out_voltage, this->mem_min_out_voltage);
-	};
-}
-
-void logic::CfgMemory::restore(void)
-{
-	this->btn_force    = this->mem_btn_force;
-	this->bmode        = this->mem_bmode;
-	this->pot_mode     = this->mem_pot_mode;
-	this->dsp_brigth   = this->mem_dsp_brigth;
-	this->dsp_dimm     = this->mem_dsp_dimm;
-	this->brake_force     = this->mem_brake_force;
-	this->max_hbrake_time = this->mem_max_hbrake_time;
-	this->lock_current    = this->mem_lock_current;
-	this->max_out_voltage = this->mem_max_out_voltage;
-	this->min_out_voltage = this->mem_min_out_voltage;
-}
-
-uint8_t logic::CfgMemory::checksum(void)
-{
-	uint32_t cs = 0;
-	cs += (uint32_t)this->mem_btn_force;
-	cs += (uint32_t)this->mem_bmode;
-	cs += (uint32_t)this->mem_pot_mode;
-	cs += (uint32_t)this->mem_dsp_brigth;
-	cs += (uint32_t)this->mem_dsp_dimm;
-	cs += (uint32_t)this->mem_brake_force;
-	cs += (uint32_t)this->mem_max_hbrake_time;
-	cs += (uint32_t)this->mem_lock_current;
-	cs += (uint32_t)this->mem_max_out_voltage;
-	cs += (uint32_t)this->mem_min_out_voltage;
-	
-	return (uint8_t)cs;
-}
-
-
-/**** Private function definitions ****/
diff --git a/firmware/src/logic/cfg_mem.h b/firmware/src/logic/cfg_mem.h
deleted file mode 100644
index 97460a4..0000000
--- a/firmware/src/logic/cfg_mem.h
+++ /dev/null
@@ -1,56 +0,0 @@
-#ifndef CONFIG_H_
-#define CONFIG_H_
-
-/**** Includes ****/
-#include <stdint.h>
-#include "../hw/button.h"
-
-namespace logic {
-
-/**** Public definitions ****/
-
-class CfgMemory
-{
-	protected:
-	uint8_t mem_btn_force;
-	uint8_t mem_bmode;
-	uint8_t mem_pot_mode;
-	uint8_t mem_dsp_brigth;
-	uint8_t mem_dsp_dimm;
-	uint8_t mem_brake_force;
-	uint16_t mem_max_hbrake_time;
-	uint16_t mem_lock_current;
-	uint16_t mem_max_out_voltage;
-	uint16_t mem_min_out_voltage;
-
-	public:
-	CfgMemory(void);
-	~CfgMemory(void);
-	
-	uint8_t cfg_good;
-	uint8_t btn_force;
-	uint8_t bmode;
-	uint8_t pot_mode;
-	uint8_t dsp_brigth;
-	uint8_t dsp_dimm;
-	uint8_t brake_force;
-	uint16_t max_hbrake_time;
-	uint16_t lock_current;
-	uint16_t max_out_voltage;
-	uint16_t min_out_voltage;
-	
-	void init(void);
-	void save(void);
-	void save_all(void);
-	void restore(void);
-	uint8_t checksum(void);
-};
-
-/**** Public function declarations ****/
-
-#ifdef TESTING
-#endif
-
-} //namespace
-
-#endif /* DCCD_FORCE_H_ */
\ No newline at end of file
diff --git a/firmware/src/logic/dccd_force.cpp b/firmware/src/logic/dccd_force.cpp
deleted file mode 100644
index cedebe4..0000000
--- a/firmware/src/logic/dccd_force.cpp
+++ /dev/null
@@ -1,84 +0,0 @@
-/**** Includes ****/
-#include "../utils/utils.h"
-#include "dccd_force.h"
-
-using namespace logic;
-
-/**** Private definitions ****/
-/**** Private constants ****/
-static const uint16_t def_max_hbrake_time = 1000;
-static const uint16_t def_brake_force = 100;
-
-/**** Private variables ****/
-/**** Private function declarations ****/
-/**** Public function definitions ****/
-logic::DccdForce::DccdForce(hw::Button* btn_mode, hw::Button* sw_hbrake, hw::Button* sw_brakes)
-{
-	this->mode = btn_mode;
-	this->handbrake = sw_hbrake;
-	this->brakes = sw_brakes;
-	
-	this->is_new = 0;
-	this->force = 0;
-	this->brake_mode = 0;
-	
-	this->max_hbrake_time = def_max_hbrake_time;
-	this->brake_force = def_brake_force;
-}
-
-logic::DccdForce::~DccdForce(void)
-{
-	return;
-}
-
-uint8_t logic::DccdForce::update(uint8_t user_force)
-{
-	// Process mode button
-	if((this->mode->is_new)&&(this->mode->state == hw::BUTTON_ON))
-	{
-		// Cycle brake mode
-		switch(this->brake_mode)
-		{
-			case 0:
-			this->brake_mode = 1;
-			
-			case 1:
-			this->brake_mode = 2;
-			
-			default:
-			this->brake_mode = 0;
-		}
-		
-		this->mode->is_new = 0;
-		this->is_new_bmode = 1;
-	};
-	
-	// Determine target force source
-	uint8_t next_force = user_force;
-	if((this->handbrake->state == hw::BUTTON_ON)&&((this->handbrake->time < this->max_hbrake_time)||(this->max_hbrake_time == 0)))
-	{
-		next_force = 0;
-	}
-	else if(this->brakes->state == hw::BUTTON_ON)
-	{
-		switch(this->brake_mode)
-		{
-			case 0:
-			next_force = 0;
-			
-			case 1:
-			next_force = user_force;
-			
-			default:
-			next_force = this->brake_force;
-		}
-	};
-	
-	if(next_force != this->force) this->is_new = 1;
-	
-	this->force = next_force;
-	
-	return  this->force;
-}
-
-/**** Private function definitions ****/
diff --git a/firmware/src/logic/dccd_force.h b/firmware/src/logic/dccd_force.h
deleted file mode 100644
index c5291af..0000000
--- a/firmware/src/logic/dccd_force.h
+++ /dev/null
@@ -1,42 +0,0 @@
-#ifndef DCCD_FORCE_H_
-#define DCCD_FORCE_H_
-
-/**** Includes ****/
-#include <stdint.h>
-#include "../hw/button.h"
-
-namespace logic {
-
-/**** Public definitions ****/
-
-class DccdForce
-{
-	protected:
-	hw::Button* mode;
-	hw::Button* handbrake;
-	hw::Button* brakes;
-	
-	public:
-	DccdForce(hw::Button* btn_mode, hw::Button* sw_hbrake, hw::Button* sw_brakes);
-	~DccdForce(void);
-	
-	uint8_t force;
-	uint8_t is_new;
-	
-	uint8_t brake_mode;
-	uint8_t is_new_bmode;
-	
-	uint16_t max_hbrake_time;
-	uint8_t brake_force;
-	
-	uint8_t update(uint8_t user_force);
-};
-
-/**** Public function declarations ****/
-
-#ifdef TESTING
-#endif
-
-} //namespace
-
-#endif /* DCCD_FORCE_H_ */
\ No newline at end of file
diff --git a/firmware/src/main.cpp b/firmware/src/main.cpp
index 96aa6c5..9dcfdd8 100644
--- a/firmware/src/main.cpp
+++ b/firmware/src/main.cpp
@@ -1,122 +1,32 @@
 /**** Includes ****/
 #include "utils/utils.h"
 
-#include "hw/devices.h"
-
-#include "logic/button_force.h"
-#include "logic/dccd_force.h"
-
-#include "logic/cfg_mem.h"
+#include "hw/dccd_hw.h"
 
 /**** Private definitions ****/
 /**** Private constants ****/
-static const uint16_t dsp_lock_bmode = 1000;
-static const uint16_t dsp_lock_force = 500;
-
 /**** Private variables ****/
-static logic::CfgMemory cfg_mem = logic::CfgMemory();
-
-static logic::ButtonForce button_force = logic::ButtonForce(&btn_up, &btn_down);
-static logic::DccdForce dccd_force = logic::DccdForce(&btn_mode, &sw_hbrake, &sw_brakes);
+hw::DccdHw dccd_hw;
 
 /**** Private function declarations ****/
 /**** Public function definitions ****/
 int main(void)
 {
-	// HW setup
-	devices_init();
+	// Setup
+	hw::DccdHw::dccdHwCfg_t hw_cfg;
 	
-	// Read saved config
-	cfg_mem.init();
+	hw_cfg.handbrake_pull_up = 0;
+	hw_cfg.speed_hall = 0;
 	
-	if(cfg_mem.cfg_good !=1 )
-	{
-		cfg_mem.btn_force = 0;
-		cfg_mem.bmode = 0;
-		cfg_mem.pot_mode = 0;
-		cfg_mem.dsp_brigth = 100;
-		cfg_mem.dsp_dimm = 50;
-		cfg_mem.brake_force = 100;
-		cfg_mem.max_hbrake_time = 1000;
-		cfg_mem.lock_current = 4200;
-		cfg_mem.max_out_voltage = 6500;
-		cfg_mem.min_out_voltage = 500;
-		cfg_mem.save_all();
-	};
-	
-	uint8_t user_force = 0;
-	
-	ccout.max_voltage = cfg_mem.max_out_voltage;
-	ccout.min_voltage = cfg_mem.min_out_voltage;
-	
-	button_force.force = cfg_mem.btn_force;
-	dccd_force.brake_mode = cfg_mem.bmode;
-	dccd_force.max_hbrake_time = cfg_mem.max_hbrake_time;
-	dccd_force.brake_force = cfg_mem.brake_force;
+	dccd_hw.init(&hw_cfg);
 	
 	// Super loop
 	while(1)
-	{				
+	{	
 		// Update inputs
-		devices_update_inputs();
+		dccd_hw.read();
 		
-		// Update user setting
-		button_force.update();
-		
-		// Select user force input
-		if(cfg_mem.pot_mode) user_force = pot.percent;
-		else user_force = button_force.force;
-		
-		// Calculate next target force
-		dccd_force.update(user_force);
-		
-		// Override force in case of fault
-		if((sup_fuse.fault)||(out_fuse.fault)) dccd_force.force = 0;
-		
-		// Convert force to current
-		ccout.max_current = util::percent_of(dccd_force.force, cfg_mem.lock_current);
-		
-		// Execute outputs
-		ccout.update();
-		
-		// Set display
-		if(dccd_force.is_new_bmode)
-		{
-			uint8_t bmode_img = 0x03;
-			switch(dccd_force.brake_mode)
-			{
-				case 1:
-				bmode_img = 0x0C;
-				break;
-				
-				case 2:
-				bmode_img = 0x30;
-				break;
-				
-				default:
-				bmode_img = 0x03;
-				break;
-			}
-			display.write(bmode_img);
-			display.set_lock(dsp_lock_bmode);
-			dccd_force.is_new_bmode = 0;
-		}
-		else if((button_force.is_new)&&(cfg_mem.pot_mode==0))
-		{
-			display.show_percent(dccd_force.force, hw::DisplayLed::LED_DSP_DOT10);
-			display.set_lock(dsp_lock_force);
-			button_force.is_new = 0;
-		}
-		else if(display.locked==0) display.show_percent(dccd_force.force, hw::DisplayLed::LED_DSP_DOT10);
-		
-		// Process dimm
-		if(sw_dimm.state == hw::BUTTON_ON) display.set_brigthness(cfg_mem.dsp_dimm);
-		else display.set_brigthness(cfg_mem.dsp_brigth);
-		
-		// Save user setting changes
-		cfg_mem.btn_force = button_force.force;
-		cfg_mem.bmode = dccd_force.brake_mode;
-		cfg_mem.save();
+		// Do something
 		
 		continue; // End of super loop
 	}
diff --git a/firmware/src/sort/cv_out.cpp b/firmware/src/sort/cv_out.cpp
new file mode 100644
index 0000000..4800e9d
--- /dev/null
+++ b/firmware/src/sort/cv_out.cpp
@@ -0,0 +1,50 @@
+/**** Includes ****/
+#include "../utils/utils.h"
+#include "cv_out.h"
+
+using namespace bsp;
+
+/**** Private definitions ****/
+/**** Private constants ****/
+/**** Private variables ****/
+/**** Private function declarations ****/
+
+/**** Public function definitions ****/
+bsp::CVOut::CVOut(void)
+{
+	return;
+}
+
+bsp::CVOut::~CVOut(void)
+{
+	return;
+}
+
+void bsp::CVOut::init(AnalogIn* ain_ch, Halfbridge* out_ch)
+{
+	this->ain_ch = ain_ch;
+	this->out_ch = out_ch;
+	
+	this->voltage = 0;
+	
+	this->disabled = 1;
+	this->process_ain = 1;
+}
+
+void bsp::CVOut::process(void)
+{
+	// Update analog input
+	if(this->process_ain) this->ain_ch->read();
+	
+	// Check to HiZ output
+	if(this->disabled)
+	{
+		this->out_ch->write_hiz();
+		return;
+	};
+	
+	// Calculate and apply necessary PWM
+	this->out_ch->write(util::sat_ratio(this->voltage, this->ain_ch->last_read));
+}
+
+/**** Private function definitions ****/
diff --git a/firmware/src/sort/cv_out.h b/firmware/src/sort/cv_out.h
new file mode 100644
index 0000000..87fd437
--- /dev/null
+++ b/firmware/src/sort/cv_out.h
@@ -0,0 +1,41 @@
+#ifndef CONST_VOLTAGE_OUTPUT_H_
+#define CONST_VOLTAGE_OUTPUT_H_
+
+/**** Includes ****/
+#include <stdint.h>
+#include "ain.h"
+#include "halfbridge.h"
+
+namespace bsp {
+
+/**** Public definitions ****/
+
+class CVOut
+{
+	public:
+	CVOut(void);
+	~CVOut(void);
+	
+	void init(AnalogIn* ain_ch, Halfbridge* out_ch);
+	
+	uint16_t voltage;
+	uint8_t disabled;
+	uint8_t process_ain;
+	
+	void process(void);
+	
+	#ifndef TESTING
+	protected:
+	#endif
+	AnalogIn* ain_ch;
+	Halfbridge* out_ch;
+};
+
+/**** Public function declarations ****/
+
+#ifdef TESTING
+#endif
+
+} //namespace
+
+#endif /* CONST_VOLTAGE_OUTPUT_H_ */
\ No newline at end of file
diff --git a/firmware/src/sort/fuse.cpp b/firmware/src/sort/fuse.cpp
new file mode 100644
index 0000000..1ed0dd1
--- /dev/null
+++ b/firmware/src/sort/fuse.cpp
@@ -0,0 +1,81 @@
+/**** Includes ****/
+#include "../utils/utils.h"
+#include "fuse.h"
+
+using namespace bsp;
+
+/**** Private definitions ****/
+/**** Private constants ****/
+/**** Private variables ****/
+/**** Private function declarations ****/
+
+/**** Public function definitions ****/
+bsp::Fuse::Fuse(void)
+{
+	return;
+}
+
+bsp::Fuse::~Fuse(void)
+{
+	return;
+}
+
+void bsp::Fuse::init(bsp::AnalogIn* ain_ch, bsp::VCounter* timerh)
+{
+	this->ain_ch = ain_ch;
+	this->timer = timer;
+	
+	this->current_lim = 0;
+	this->hold_time = 0;
+	this->cooldown_time = 0;
+	
+	this->process_ain = 1;
+	this->auto_reset = 0;
+	
+	this->warning = 0;
+	this->fault = 0;
+	
+	this->ts_oc_chnage = 0;
+}
+
+void bsp::Fuse::process(void)
+{
+	// Update analog input
+	if(this->process_ain) this->ain_ch->read();
+	
+	// Get current time
+	uint16_t ts_now = this->timer->read();
+	
+	// Update over current and warning condition
+	uint8_t is_oc = 0;
+	if(this->ain_ch->last_read >= this->current_lim) is_oc = 1;
+	
+	// Note start time if new OC condition
+	if(is_oc != this->warning) this->ts_oc_chnage = ts_now;
+	
+	// Update warning
+	this->warning = is_oc;
+	
+	// Calculate OC condition time
+	uint16_t td = util::time_delta(this->ts_oc_chnage, ts_now);
+	uint32_t time_ms = this->timer->convert_ms(td);
+	
+	// Check for fault set
+	if((this->fault==0)&&(time_ms > (uint32_t)this->hold_time))
+	{
+		this->fault = 1;
+		return;
+	};
+	
+	// Check if allowed auto reset
+	if((this->auto_reset==0)||(this->cooldown_time==0)) return;
+	
+	// Check for fault reset
+	if((this->fault!=0)&&(time_ms > (uint32_t)this->cooldown_time))
+	{
+		this->fault = 0;
+		return;
+	};
+}
+
+/**** Private function definitions ****/
diff --git a/firmware/src/sort/fuse.h b/firmware/src/sort/fuse.h
new file mode 100644
index 0000000..23189b6
--- /dev/null
+++ b/firmware/src/sort/fuse.h
@@ -0,0 +1,48 @@
+#ifndef FUSE_H_
+#define FUSE_H_
+
+/**** Includes ****/
+#include <stdint.h>
+#include "ain.h"
+#include "vcounter.h"
+
+namespace bsp {
+
+/**** Public definitions ****/
+
+class Fuse
+{
+	public:
+	Fuse(void);
+	~Fuse(void);
+	
+	void init(AnalogIn* ain_ch, VCounter* timer);
+	
+	uint8_t warning;
+	uint8_t fault;
+	
+	uint16_t current_lim;
+	uint16_t hold_time;
+	uint16_t cooldown_time;
+	
+	uint8_t process_ain;
+	uint8_t auto_reset;
+	
+	void process(void);
+	
+	#ifndef TESTING
+	protected:
+	#endif
+	AnalogIn* ain_ch;
+	VCounter* timer;
+	uint16_t ts_oc_chnage;
+};
+
+/**** Public function declarations ****/
+
+#ifdef TESTING
+#endif
+
+} //namespace
+
+#endif /* POTENTIOMETER_H_ */
\ No newline at end of file
diff --git a/firmware/src/sort/voltlock.cpp b/firmware/src/sort/voltlock.cpp
new file mode 100644
index 0000000..be25a93
--- /dev/null
+++ b/firmware/src/sort/voltlock.cpp
@@ -0,0 +1,83 @@
+/**** Includes ****/
+#include "../utils/utils.h"
+#include "voltlock.h"
+
+using namespace bsp;
+
+/**** Private definitions ****/
+/**** Private constants ****/
+/**** Private variables ****/
+/**** Private function declarations ****/
+
+/**** Public function definitions ****/
+bsp::VoltLock::VoltLock(void)
+{
+	return;
+}
+
+bsp::VoltLock::~VoltLock(void)
+{
+	return;
+}
+
+void bsp::VoltLock::init(bsp::AnalogIn* ain_ch, bsp::VCounter* timerh)
+{
+	this->ain_ch = ain_ch;
+	this->timer = timer;
+	
+	this->undervoltage_lim = 0xFFFF;
+	this->overvoltage_lim = 0;
+	this->hold_time = 0;
+	this->cooldown_time = 0;
+	
+	this->process_ain = 1;
+	this->auto_reset = 1;
+	
+	this->warning = 0;
+	this->fault = 0;
+	
+	this->ts_oc_chnage = 0;
+}
+
+void bsp::VoltLock::process(void)
+{
+	// Update analog input
+	if(this->process_ain) this->ain_ch->read();
+	
+	// Get current time
+	uint16_t ts_now = this->timer->read();
+	
+	// Update over current and warning condition
+	uint8_t is_outside = 0;
+	if(this->ain_ch->last_read <= this->undervoltage_lim) is_outside = 1;
+	if(this->ain_ch->last_read >= this->overvoltage_lim) is_outside = 1;
+	
+	// Note start time if new OC condition
+	if(is_outside != this->warning) this->ts_oc_chnage = ts_now;
+	
+	// Update warning
+	this->warning = is_outside;
+	
+	// Calculate warning condition time
+	uint16_t td = util::time_delta(this->ts_oc_chnage, ts_now);
+	uint32_t time_ms = this->timer->convert_ms(td);
+	
+	// Check for fault set
+	if((this->fault==0)&&(time_ms > (uint32_t)this->hold_time))
+	{
+		this->fault = 1;
+		return;
+	};
+	
+	// Check if allowed auto reset
+	if(this->auto_reset==0) return;
+	
+	// Check for fault reset
+	if((this->fault!=0)&&(time_ms > (uint32_t)this->cooldown_time))
+	{
+		this->fault = 0;
+		return;
+	};
+}
+
+/**** Private function definitions ****/
diff --git a/firmware/src/sort/voltlock.h b/firmware/src/sort/voltlock.h
new file mode 100644
index 0000000..f22f941
--- /dev/null
+++ b/firmware/src/sort/voltlock.h
@@ -0,0 +1,49 @@
+#ifndef VOLTAGE_LOCKOUT_H_
+#define VOLTAGE_LOCKOUT_H_
+
+/**** Includes ****/
+#include <stdint.h>
+#include "ain.h"
+#include "vcounter.h"
+
+namespace bsp {
+
+/**** Public definitions ****/
+
+class VoltLock
+{
+	public:
+	VoltLock(void);
+	~VoltLock(void);
+	
+	void init(AnalogIn* ain_ch, VCounter* timer);
+	
+	uint8_t warning;
+	uint8_t fault;
+	
+	uint16_t undervoltage_lim;
+	uint16_t overvoltage_lim;
+	uint16_t hold_time;
+	uint16_t cooldown_time;
+	
+	uint8_t process_ain;
+	uint8_t auto_reset;
+	
+	void process(void);
+	
+	#ifndef TESTING
+	protected:
+	#endif
+	AnalogIn* ain_ch;
+	VCounter* timer;
+	uint16_t ts_oc_chnage;
+};
+
+/**** Public function declarations ****/
+
+#ifdef TESTING
+#endif
+
+} //namespace
+
+#endif /* VOLTAGE_LOCKOUT_H_ */
\ No newline at end of file
diff --git a/firmware/src/uDCCD.cppproj b/firmware/src/uDCCD.cppproj
index f730fb9..64e52e2 100644
--- a/firmware/src/uDCCD.cppproj
+++ b/firmware/src/uDCCD.cppproj
@@ -153,12 +153,78 @@
     </ToolchainSettings>
   </PropertyGroup>
   <ItemGroup>
+    <Compile Include="bsp\ain.cpp">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="bsp\ain.h">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="bsp\ain_lpf.cpp">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="bsp\ain_lpf.h">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="bsp\board.cpp">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="bsp\board.h">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="bsp\din.cpp">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="bsp\din.h">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="bsp\dout.cpp">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="bsp\dout.h">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="bsp\pwm_out.cpp">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="bsp\pwm_out.h">
+      <SubType>compile</SubType>
+    </Compile>
     <Compile Include="bsp\mcu\mcu_hal.h">
       <SubType>compile</SubType>
     </Compile>
     <Compile Include="bsp\mcu\mcu_hal_r8.cpp">
       <SubType>compile</SubType>
     </Compile>
+    <Compile Include="hw\button.cpp">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="hw\button.h">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="hw\dccd_hw.cpp">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="hw\dccd_hw.h">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="hw\led_display.cpp">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="hw\led_display.h">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="hw\out_driver.cpp">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="hw\out_driver.h">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="hw\potentiometer.cpp">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="hw\potentiometer.h">
+      <SubType>compile</SubType>
+    </Compile>
     <Compile Include="main.cpp">
       <SubType>compile</SubType>
     </Compile>
@@ -174,10 +240,17 @@
     <Compile Include="utils\utils.h">
       <SubType>compile</SubType>
     </Compile>
+    <Compile Include="utils\vcounter.cpp">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="utils\vcounter.h">
+      <SubType>compile</SubType>
+    </Compile>
   </ItemGroup>
   <ItemGroup>
     <Folder Include="bsp" />
     <Folder Include="bsp\mcu" />
+    <Folder Include="hw" />
     <Folder Include="utils" />
   </ItemGroup>
   <Import Project="$(AVRSTUDIO_EXE_PATH)\\Vs\\Compiler.targets" />
diff --git a/firmware/src/utils/utils.cpp b/firmware/src/utils/utils.cpp
index b4801bc..ad9e02c 100644
--- a/firmware/src/utils/utils.cpp
+++ b/firmware/src/utils/utils.cpp
@@ -17,6 +17,18 @@ uint8_t util::invert(uint8_t x)
 	else return 1;
 }
 
+uint16_t util::invert(uint16_t x)
+{
+	if(x!=0) return 0;
+	else return 1;
+}
+
+uint32_t util::invert(uint32_t x)
+{
+	if(x!=0) return 0;
+	else return 1;
+}
+
 uint8_t util::sat_add(uint8_t x, uint8_t y)
 {
 	uint8_t z = x + y;
@@ -97,6 +109,46 @@ uint16_t util::sat_cast(int32_t x)
 	else return (uint16_t)x;
 }
 
+uint8_t util::is_timed_out(uint16_t time, uint16_t limit)
+{
+	if(time >= limit) return 1;
+	else return 0;
+}
+
+uint8_t util::is_in_range(uint16_t value, uint16_t min, uint16_t max)
+{
+	if((value >= min)&&(value <= max)) return 1;
+	else return 0;
+}
+
+uint16_t util::time_delta(uint16_t start, uint16_t end)
+{
+	if(end >= start) return (end-start);
+	uint16_t temp = 0xFFFF - start;
+	return temp + end;
+}
+
+uint32_t util::time_delta(uint32_t start, uint32_t end)
+{
+	if(end >= start) return (end-start);
+	uint32_t temp = 0xFFFFFFFF - start;
+	return temp + end;
+}
+
+uint16_t util::time_delta(uint16_t start, uint16_t end, uint16_t max)
+{
+	if(end >= start) return (end-start);
+	uint16_t temp = max - start;
+	return temp + end;
+}
+
+uint32_t util::time_delta(uint32_t start, uint32_t end, uint32_t max)
+{
+	if(end >= start) return (end-start);
+	uint32_t temp = max - start;
+	return temp + end;
+}
+
 uint16_t util::convert_muldivoff(uint16_t raw, uint8_t mul, uint8_t div, int16_t offset)
 {
 	int32_t temp = (int32_t)raw;
diff --git a/firmware/src/utils/utils.h b/firmware/src/utils/utils.h
index 9f16bb9..12d6687 100644
--- a/firmware/src/utils/utils.h
+++ b/firmware/src/utils/utils.h
@@ -9,17 +9,8 @@ namespace util {
 /**** Public definitions ****/
 /**** Public function declarations ****/
 uint8_t invert(uint8_t x);
-
-uint16_t sat_cast(uint32_t x);
-uint16_t sat_cast(int32_t x);
-
-uint16_t convert_muldivoff(uint16_t raw, uint8_t mul, uint8_t div, int16_t offset);
-uint16_t sat_mul_kilo(uint16_t xk, uint16_t yk);
-uint16_t sat_div_kilo(uint16_t top, uint16_t bot);
-uint16_t sat_ratio(uint16_t top, uint16_t bot);
-uint16_t percent_to_16b(uint8_t percent);
-
-uint16_t percent_of(uint8_t percent, uint16_t value);
+uint16_t invert(uint16_t x);
+uint32_t invert(uint32_t x);
 
 uint8_t sat_add(uint8_t x, uint8_t y);
 uint16_t sat_add(uint16_t x, uint16_t y);
@@ -33,8 +24,25 @@ uint8_t abs_subtract(uint8_t x, uint8_t y);
 uint16_t abs_subtract(uint16_t x, uint16_t y);
 uint32_t abs_subtract(uint32_t x, uint32_t y);
 
-uint16_t interpolate_1d(uint16_t x, uint16_t* x_axis, uint16_t* y_values, uint8_t len_axis);
-uint16_t interpolate_2d(uint16_t x, uint16_t y, uint16_t* x_axis, uint8_t len_x_axis, uint16_t* y_axis, uint8_t len_y_axis, uint16_t* z_values);
+uint16_t sat_cast(uint32_t x);
+uint16_t sat_cast(int32_t x);
+
+uint8_t is_timed_out(uint16_t time, uint16_t limit);
+uint8_t is_in_range(uint16_t value, uint16_t min, uint16_t max);
+
+uint16_t time_delta(uint16_t start, uint16_t end);
+uint32_t time_delta(uint32_t start, uint32_t end);
+
+uint16_t time_delta(uint16_t start, uint16_t end, uint16_t max);
+uint32_t time_delta(uint32_t start, uint32_t end, uint32_t max);
+
+uint16_t convert_muldivoff(uint16_t raw, uint8_t mul, uint8_t div, int16_t offset);
+uint16_t sat_mul_kilo(uint16_t xk, uint16_t yk);
+uint16_t sat_div_kilo(uint16_t top, uint16_t bot);
+uint16_t sat_ratio(uint16_t top, uint16_t bot);
+uint16_t percent_to_16b(uint8_t percent);
+
+uint16_t percent_of(uint8_t percent, uint16_t value);
 
 #ifdef TESTING
 #endif
diff --git a/firmware/src/utils/vcounter.cpp b/firmware/src/utils/vcounter.cpp
new file mode 100644
index 0000000..6870cc1
--- /dev/null
+++ b/firmware/src/utils/vcounter.cpp
@@ -0,0 +1,67 @@
+/**** Includes ****/
+#include "utils.h"
+#include "vcounter.h"
+
+using namespace util;
+
+/**** Private definitions ****/
+/**** Private constants ****/
+/**** Private variables ****/
+/**** Private function declarations ****/
+/**** Public function definitions ****/
+util::VCounter::VCounter(void)
+{
+	return;
+}
+
+util::VCounter::~VCounter(void)
+{
+	return;
+}
+
+void util::VCounter::init(uint16_t top, uint16_t step_us)
+{
+	this->counter = 0;
+	this->top = top;
+	this->step_us = step_us;
+	this->disabled = 1;
+}
+
+void util::VCounter::reset(void)
+{
+	this->counter = 0;
+}
+
+void util::VCounter::increment(void)
+{
+	if(this->disabled) return;
+	this->counter++;
+	if(this->counter > this->top) this->counter = 0;
+}
+
+uint16_t util::VCounter::read(void)
+{
+	return this->counter;
+}
+
+uint32_t util::VCounter::read_ms(void)
+{
+	return this->convert_ms(this->counter);
+}
+
+uint16_t util::VCounter::read_top(void)
+{
+	return this->top;
+}
+
+uint32_t util::VCounter::convert_ms(uint16_t raw)
+{
+	if(this->step_us==0) return 0;
+	
+	uint32_t out = (uint32_t)raw * (uint32_t)this->step_us;
+	return out/1000;
+}
+
+
+/**** Private function definitions ****/
+
diff --git a/firmware/src/utils/vcounter.h b/firmware/src/utils/vcounter.h
new file mode 100644
index 0000000..557bac8
--- /dev/null
+++ b/firmware/src/utils/vcounter.h
@@ -0,0 +1,42 @@
+#ifndef VIRTUAL_COUNTER_H_
+#define VIRTUAL_COUNTER_H_
+
+/**** Includes ****/
+#include <stdint.h>
+
+namespace util {
+
+/**** Public definitions ****/
+class VCounter
+{	
+	public:
+	VCounter(void);
+	~VCounter(void);
+	
+	void init(uint16_t top, uint16_t step_us);
+	
+	uint8_t disabled;
+	
+	void reset(void);
+	void increment(void);
+	uint16_t read(void);
+	uint32_t read_ms(void);
+	uint16_t read_top(void);
+	uint32_t convert_ms(uint16_t raw);
+	
+	#ifndef TESTING
+	protected:
+	#endif
+	uint16_t step_us;
+	uint16_t counter;
+	uint16_t top;
+};
+
+/**** Public function declarations ****/
+
+#ifdef TESTING
+#endif
+
+} //namespace
+
+#endif /* VIRTUAL_COUNTER_H_ */
\ No newline at end of file