Changes

Started coil driver class
Created pot class
2024-04-10 15:46:26 +03:00 · 2024-04-10 15:46:17 +03:00 · 2024-04-10 15:46:07 +03:00 · 2024-04-10 15:46:00 +03:00 · 2024-04-10 15:45:50 +03:00 · 2024-04-10 15:45:32 +03:00
55 changed files with 1347 additions and 3025 deletions
--- a/firmware/src/board/ain.cpp
+++ b/firmware/src/board/ain.cpp
@@ -3,24 +3,15 @@
 #include "mcu/mcu_hal.h"
 #include "ain.h"

-using namespace bsp;
+using namespace board;

 /**** Private definitions ****/
 /**** Private constants ****/
 /**** Private variables ****/
 /**** Private function declarations ****/
+
 /**** Public function definitions ****/
-bsp::AnalogIn::AnalogIn(void)
-{
-	return;
-}
-
-bsp::AnalogIn::~AnalogIn(void)
-{
-	return;
-}
-
-void bsp::AnalogIn::init(uint8_t adc_ch)
+board::AnalogIn::AnalogIn(uint8_t adc_ch)
 {
 	this->adc_ch = adc_ch;
 	this->mul = DEF_AIN_MUL;
@@ -29,8 +20,7 @@ void bsp::AnalogIn::init(uint8_t adc_ch)
 	this->last_read = 0;
 }

-
-uint16_t bsp::AnalogIn::read(void)
+uint16_t board::AnalogIn::read(void)
 {
 	//Read ADC
 	uint16_t raw = mcu::adc_read(this->adc_ch);
--- a/firmware/src/board/ain.h
+++ b/firmware/src/board/ain.h
@@ -4,7 +4,7 @@
 /**** Includes ****/
 #include <stdint.h>

-namespace bsp {
+namespace board {

 /**** Public definitions ****/
 static const uint8_t DEF_AIN_MUL = 215;
@@ -12,12 +12,12 @@ static const uint8_t DEF_AIN_DIV = 44;
 static const int16_t DEF_AIN_OFFSET = 0;

 class AnalogIn
-{	
-	public:
-	AnalogIn(void);
-	~AnalogIn(void);
+{
+	protected:
+	uint8_t adc_ch;
 	
-	void init(uint8_t adc_ch);
+	public:
+	AnalogIn(uint8_t adc_ch);
 	
 	uint8_t mul;
 	uint8_t div;
@@ -25,11 +25,6 @@ class AnalogIn
 	uint16_t last_read;
 	
 	uint16_t read(void);
-	
-	#ifndef TESTING
-	protected:
-	#endif
-	uint8_t adc_ch;
 };

 /**** Public function declarations ****/
--- a/firmware/src/board/din.cpp
+++ b/firmware/src/board/din.cpp
@@ -0,0 +1,40 @@
+/**** Includes ****/
+#include "../utils/utils.h"
+#include "mcu/mcu_hal.h"
+#include "din.h"
+
+using namespace board;
+
+/**** Private definitions ****/
+/**** Private constants ****/
+/**** Private variables ****/
+/**** Private function declarations ****/
+
+/**** Public function definitions ****/
+board::DigitalIn::DigitalIn(uint8_t gpio_ch, uint8_t inverted, uint8_t init_value)
+{
+	this->gpio_ch = gpio_ch;
+	this->invert = inverted;
+	
+	if(init_value) this->last_read = DIN_HIGH;
+	else this->last_read = DIN_LOW;
+}
+
+board::DigitalIn::~DigitalIn(void)
+{
+	return;
+}
+
+uint8_t board::DigitalIn::read(void)
+{
+	uint8_t lvl = 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;
+	
+	return this->last_read;
+}
+
+/**** Private function definitions ****/
--- a/firmware/src/board/din.h
+++ b/firmware/src/board/din.h
@@ -1,29 +1,28 @@
-#ifndef DIGITAL_IN_H_
-#define DIGITAL_IN_H_
+#ifndef DIGITAL_INPUT_H_
+#define DIGITAL_INPUT_H_

 /**** Includes ****/
 #include <stdint.h>

-namespace bsp {
+namespace board {

 /**** Public definitions ****/
+const uint8_t DIN_LOW  = 0;
+const uint8_t DIN_HIGH = 1;
+
 class DigitalIn
-{	
-	public:
-	DigitalIn(void);
-	~DigitalIn(void);
+{
+	protected:
+	uint8_t gpio_ch;
+	uint8_t invert;
 	
-	void init(uint8_t gpio_ch, uint8_t inverted);
+	public:
+	DigitalIn(uint8_t gpio_ch, uint8_t inverted, uint8_t init_value);
+	~DigitalIn(void);
 	
 	uint8_t last_read;
 	
 	uint8_t read(void);
-	
-	#ifndef TESTING
-	protected:
-	#endif
-	uint8_t gpio_ch;
-	uint8_t is_inverted;
 };

 /**** Public function declarations ****/
@@ -33,4 +32,4 @@ class DigitalIn

 } //namespace

-#endif /* DIGITAL_IN_H_ */
+#endif /* DIGITAL_INPUT_H_ */
--- a/firmware/src/board/dio.cpp
+++ b/firmware/src/board/dio.cpp
@@ -0,0 +1,34 @@
+/**** Includes ****/
+#include "../utils/utils.h"
+#include "mcu/mcu_hal.h"
+#include "dio.h"
+
+using namespace board;
+
+/**** Private definitions ****/
+/**** Private constants ****/
+/**** Private variables ****/
+/**** Private function declarations ****/
+
+/**** Public function definitions ****/
+board::DigitalIO::DigitalIO(uint8_t gpio_ch, uint8_t init_value) : DigitalIn(gpio_ch, 0, init_value), DigitalOut(gpio_ch, 0)
+{
+	return;
+}
+
+board::DigitalIO::~DigitalIO(void)
+{
+	this->write(DOUT_HIZ);
+}
+
+uint8_t board::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 ****/
--- a/firmware/src/board/dio.h
+++ b/firmware/src/board/dio.h
@@ -0,0 +1,32 @@
+#ifndef DIGITAL_IO_H_
+#define DIGITAL_IO_H_
+
+/**** Includes ****/
+#include <stdint.h>
+#include "din.h"
+#include "dout.h"
+
+namespace board {
+
+/**** 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_ */
--- a/firmware/src/board/dout.cpp
+++ b/firmware/src/board/dout.cpp
@@ -0,0 +1,52 @@
+/**** Includes ****/
+#include "../utils/utils.h"
+#include "mcu/mcu_hal.h"
+#include "dout.h"
+
+using namespace board;
+
+/**** Private definitions ****/
+/**** Private constants ****/
+/**** Private variables ****/
+/**** Private function declarations ****/
+
+/**** Public function definitions ****/
+board::DigitalOut::DigitalOut(uint8_t gpio_ch, uint8_t inverted)
+{
+	this->gpio_ch = gpio_ch;
+	this->invert = inverted;
+	this->write(DOUT_HIZ);
+}
+
+board::DigitalOut::~DigitalOut(void)
+{
+	this->write(DOUT_HIZ);
+}
+
+void board::DigitalOut::write(int8_t level)
+{
+	if(level > 0)
+	{
+		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 board::DigitalOut::get_set_level(void)
+{
+	return this->last_set;
+}
+
+/**** Private function definitions ****/
--- a/firmware/src/board/dout.h
+++ b/firmware/src/board/dout.h
@@ -0,0 +1,36 @@
+#ifndef DIGITAL_OUTPUT_H_
+#define DIGITAL_OUTPUT_H_
+
+/**** Includes ****/
+#include <stdint.h>
+
+namespace board {
+
+/**** 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;
+	
+	public:
+	DigitalOut(uint8_t gpio_ch, uint8_t inverted);
+	~DigitalOut(void);
+	
+	void write(int8_t level);
+	int8_t get_set_level(void);
+};
+
+/**** Public function declarations ****/
+
+#ifdef TESTING
+#endif
+
+} //namespace
+
+#endif /* DIGITAL_OUTPUT_H_ */
--- a/firmware/src/board/halfbridge.cpp
+++ b/firmware/src/board/halfbridge.cpp
@@ -0,0 +1,68 @@
+/**** Includes ****/
+#include "../utils/utils.h"
+#include "mcu/mcu_hal.h"
+#include "halfbridge.h"
+
+using namespace board;
+
+/**** Private definitions ****/
+/**** Private constants ****/
+/**** Private variables ****/
+/**** Private function declarations ****/
+
+/**** Public function definitions ****/
+board::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) this->max_dc = 100;
+	else this->max_dc = max_dc;
+	this->disable();
+}
+
+board::Hafbridge::~Hafbridge(void)
+{
+	this->last_duty = 0;
+	this->disable();
+}
+
+void board::Hafbridge::write(uint8_t duty)
+{
+	// Limit duty
+	if(duty > this->max_dc) duty = this->max_dc;
+	this->last_duty = duty;
+	
+	if(this->enabled == 0) return;
+	
+	// Convert percent to 16b duty cycle
+	uint16_t dc = util::percent_to_16b(duty);
+	// Set PWM
+	mcu::pwm_write(this->pwm_ch, dc);
+}
+
+void board::Hafbridge::enable(void)
+{
+	mcu::gpio_write(this->gpio_ch, mcu::LEVEL_HIGH);
+	this->enabled = 1;
+	this->write(this->last_duty);
+}
+
+void board::Hafbridge::disable(void)
+{
+	mcu::pwm_write(this->pwm_ch, 0);
+	mcu::gpio_write(this->gpio_ch, mcu::LEVEL_LOW);
+	this->enabled = 0;
+}
+
+uint8_t board::Hafbridge::get_set_duty(void)
+{
+	return this->last_duty;
+}
+
+uint8_t board::Hafbridge::is_enabled(void)
+{
+	return this->enabled;
+}
+
+/**** Private function definitions ****/
--- a/firmware/src/board/halfbridge.h
+++ b/firmware/src/board/halfbridge.h
@@ -0,0 +1,37 @@
+#ifndef HALFBRIDGE_H_
+#define HALFBRIDGE_H_
+
+/**** Includes ****/
+#include <stdint.h>
+
+namespace board {
+
+/**** Public definitions ****/
+class Hafbridge
+{
+	protected:
+	uint8_t pwm_ch;
+	uint8_t gpio_ch;
+	uint8_t last_duty;
+	uint8_t enabled;
+	uint8_t max_dc;
+	
+	public:
+	Hafbridge(uint8_t hs_pwm_ch, uint8_t ls_gpio_ch, uint8_t max_dc);
+	~Hafbridge(void);
+	
+	void write(uint8_t duty);
+	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_ */
--- a/firmware/src/board/mcu/mcu_hal.h
+++ b/firmware/src/board/mcu/mcu_hal.h
@@ -0,0 +1,125 @@
+#ifndef MCU_HAL_H_
+#define MCU_HAL_H_
+
+/**** Includes ****/
+#include <stdint.h>
+
+namespace mcu {
+
+/**** Public definitions ****/
+/*
+GPIO0	Down
+GPIO1	Up
+GPIO2	Mode
+GPIO3	Handbrake
+GPIO4	Brakes
+GPIO5	Dimm
+GPIO6	LED0
+GPIO7	LED1
+GPIO8	LED2
+GPIO9	LED3
+GPIO10	LED4
+GPIO11	LED5
+GPIO12	DCCD Enable
+GPIO13	Handbrake pull
+GPIO14	Speed pull
+GPIO15	DCCD PWM
+GPIO16	LED PWM
+
+ADC0	Output current
+ADC1	Output voltage
+ADC2	Battery current
+ADC3	Battery voltage
+ADC4	Potentiometer
+ADC5	Mode
+ADC8	MCU temperature
+ADC14	MCU internal reference
+ADC15	MCU ground
+*/
+
+const uint8_t GPIO0 = 0;	//PC5 Mode
+const uint8_t GPIO1 = 1;	//PC4 Pot  
+const uint8_t GPIO2 = 2;	//PE1 Down
+const uint8_t GPIO3 = 3;	//PE3 Up
+const uint8_t GPIO4 = 4;	//PD7 Dimm
+const uint8_t GPIO5 = 5;	//PB7 Brakes
+const uint8_t GPIO6 = 6;	//PB6 Handbrake
+const uint8_t GPIO7 = 7;	//PB5 Handbrake pull
+const uint8_t GPIO8 = 8;	//PD6 Speed pull
+const uint8_t GPIO9 = 9;	//PD0 LED0
+const uint8_t GPIO10 = 10;	//PD1 LED1
+const uint8_t GPIO11 = 11;	//PD2 LED2
+const uint8_t GPIO12 = 12;	//PD3 LED3
+const uint8_t GPIO13 = 13;	//PD4 LED4
+const uint8_t GPIO14 = 14;	//PD5 LED5
+const uint8_t GPIO15 = 15;	//PB0 DCCD Enable
+const uint8_t GPIO16 = 16;	//PB1 DCCD PWM
+const uint8_t GPIO17 = 17;	//PB2 LED PWM
+
+const uint8_t LEVEL_LOW  = 0;
+const uint8_t LEVEL_HIGH = 1;
+const int8_t LEVEL_HIZ   = -1;
+
+const uint8_t ADC0 = 0;	//Output current
+const uint8_t ADC1 = 1;	//Output voltage
+const uint8_t ADC2 = 2;	//Battery voltage
+const uint8_t ADC3 = 3;	//Battery current
+const uint8_t ADC4 = 4;	//Potentiometer
+const uint8_t ADC5 = 5;	//Mode
+const uint8_t ADC8 = 8;	//MCU temperature
+const uint8_t ADC14 = 14;	//MCU internal reference
+const uint8_t ADC15 = 15;	//MCU ground
+
+const uint8_t PWM0 = 0;	//DCCD
+const uint8_t PWM1 = 1;	//LED
+
+//ADC definitions
+typedef enum {
+	ADC_DIV2 = 0x01,
+	ADC_DIV4 = 0x02,
+	ADC_DIV8 = 0x03,
+	ADC_DIV16 = 0x04,
+	ADC_DIV32 = 0x05,
+	ADC_DIV64 = 0x06,
+	ADC_DIV128 = 0x07
+} adcClkDiv_t;
+
+//Timer definitions
+typedef enum {
+	TIM_DIV1 = 0x01,
+	TIM_DIV8 = 0x02,
+	TIM_DIV64 = 0x03,
+	TIM_DIV256 = 0x04,
+	TIM_DIV1024 = 0x05
+} timerClkDiv_t;
+
+typedef struct {
+	adcClkDiv_t adc_clk;
+	timerClkDiv_t pwm_clk;
+	uint16_t pwm_top;
+	uint8_t pwm_ch1_en;
+} startupCfg_t;
+
+/**** Public function declarations ****/
+void startup(startupCfg_t* hwCfg);
+
+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);
+
+uint16_t adc_read(uint8_t ch);
+
+void pwm_write(uint8_t ch, uint16_t dc);
+uint16_t pwm_read(uint8_t ch);
+
+uint8_t eeprom_read8b(uint16_t address);
+uint16_t eeprom_read16b(uint16_t address);
+uint32_t eeprom_read32b(uint16_t address);
+
+void eeprom_write8b(uint16_t address, uint8_t value);
+void eeprom_write16b(uint16_t address, uint16_t value);
+void eeprom_write32b(uint16_t address, uint32_t value);
+
+} //namespace
+
+#endif /* MCU_HAL_H_ */
--- a/firmware/src/board/mcu/mcu_hal_r8.cpp
+++ b/firmware/src/board/mcu/mcu_hal_r8.cpp
@@ -2,17 +2,12 @@
 #include <avr/io.h>
 #include <avr/eeprom.h>
 #include "mcu_hal.h"
-#include <util/delay.h>
-
-#define F_CPU 8000000UL

 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);
@@ -39,9 +34,9 @@ void mcu::startup(startupCfg_t* hwCfg)
 	DDRB |= 0x03;   //Set as output
 	
 	// Common OD PWM pin
-	if(hwCfg->od_common_is_pwm) PORTB &= ~0x04; //Set low
-	else PORTB |= 0x04;                         //Set high
-	DDRB |= 0x04;                               //Set as output
+	if(hwCfg->pwm_ch1_en) PORTB &= ~0x04; //Set low
+	else PORTB |= 0x04;                   //Set high
+	DDRB |= 0x04;                         //Set as output
 	
 	// OD control pins
 	PORTD &= ~0x3F; //Set low (off)
@@ -71,10 +66,6 @@ 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
@@ -89,7 +80,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->od_common_is_pwm) TCCR1A |= 0x30; //Connect OC1B, inverted mode
+	if(hwCfg->pwm_ch1_en) TCCR1A |= 0x30; //Connect OC1B, inverted mode
 	TCCR1B = 0x18; //PWM, Phase & Frequency Correct ICR1 top, no clock, WGM:0xE
 	TCCR1C = 0x00;
 	TCNT1 = 0x0000;
@@ -103,309 +94,7 @@ void mcu::startup(startupCfg_t* hwCfg)
 	TCCR1B |= tim1_prescaler;  //Enable timer
 }

-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;
-}
-
+// ADC Interface functions 
 uint16_t mcu::adc_read(uint8_t ch)
 {
 	//check if ADC is enabled
@@ -423,7 +112,7 @@ uint16_t mcu::adc_read(uint8_t ch)
 	return ADC;
 }

-// PWM interface functions
+// PWM Timer Interface functions
 void mcu::pwm_write(uint8_t ch, uint16_t dc)
 {
 	dc = 0xFFFF - dc;
@@ -459,7 +148,252 @@ uint16_t mcu::pwm_read(uint8_t ch)
 	return (uint16_t)temp;
 }

-// EEPROM interface functions
+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;
+	}
+}
+
 uint8_t mcu::eeprom_read8b(uint16_t address)
 {
 	return eeprom_read_byte((uint8_t*)address);
@@ -478,19 +412,16 @@ uint32_t mcu::eeprom_read32b(uint16_t address)
 void mcu::eeprom_write8b(uint16_t address, uint8_t value)
 {
 	eeprom_write_byte((uint8_t*)address, value);
-	_delay_ms(5);
 }

 void mcu::eeprom_write16b(uint16_t address, uint16_t value)
 {
 	eeprom_write_word((uint16_t*)address, value);
-	_delay_ms(5);
 }

 void mcu::eeprom_write32b(uint16_t address, uint32_t value)
 {
 	eeprom_write_dword((uint32_t*)address, value);
-	_delay_ms(5);
 }

 /**** Private function definitions ****/
@@ -504,11 +435,11 @@ static void pwm_write_ocx(uint8_t ch, uint16_t value)
 {
 	switch(ch)
 	{
-		case PWM_OUT:
+		case PWM0:
 		OCR1A = value;
 		return;
 		
-		case PWM_OD:
+		case PWM1:
 		OCR1B = value;
 		return;
 		
@@ -521,10 +452,10 @@ static uint16_t pwm_read_ocx(uint8_t ch)
 {
 	switch(ch)
 	{
-		case PWM_OUT:
+		case PWM0:
 		return OCR1A;
 		
-		case PWM_OD:
+		case PWM1:
 		return OCR1B ;
 		
 		default:
--- a/firmware/src/board/pwm.cpp
+++ b/firmware/src/board/pwm.cpp
@@ -0,0 +1,40 @@
+/**** Includes ****/
+#include "../utils/utils.h"
+#include "mcu/mcu_hal.h"
+#include "pwm.h"
+
+using namespace board;
+
+/**** Private definitions ****/
+/**** Private constants ****/
+/**** Private variables ****/
+/**** Private function declarations ****/
+
+/**** Public function definitions ****/
+board::PWMout::PWMout(uint8_t pwm_ch)
+{
+	this->pwm_ch = pwm_ch;
+	this->write(0);
+}
+
+board::PWMout::~PWMout(void)
+{
+	this->write(0);
+}
+
+void board::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 board::PWMout::get_set_duty(void)
+{
+	return this->last_duty;
+}
+
+/**** Private function definitions ****/
--- a/firmware/src/board/pwm.h
+++ b/firmware/src/board/pwm.h
@@ -0,0 +1,31 @@
+#ifndef PWM_H_
+#define PWM_H_
+
+/**** Includes ****/
+#include <stdint.h>
+
+namespace board {
+
+/**** 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_ */
--- a/firmware/src/board/udccd_board.h
+++ b/firmware/src/board/udccd_board.h
@@ -0,0 +1,44 @@
+#ifndef UDCCD_BOARD_H_
+#define UDCCD_BOARD_H_
+
+/**** Includes ****/
+#include <stdint.h>
+
+using namespace board;
+
+/**** Public definitions ****/
+static AnalogIn dccd_i(mcu::ADC0);
+static AnalogIn dccd_u(mcu::ADC1);
+static AnalogIn bat_u(mcu::ADC2);
+static AnalogIn bat_i(mcu::ADC3);
+
+static Hafbridge hbridge(mcu::PWM0, mcu::GPIO15, 95);
+
+static AnalogIn ain1(mcu::ADC5);
+static AnalogIn ain2(mcu::ADC4);
+
+static DigitalIn din1(mcu::GPIO0, 0, board::DIN_HIGH);
+static DigitalIn din2(mcu::GPIO1, 0, board::DIN_HIGH);
+static DigitalIn din3(mcu::GPIO2, 0, board::DIN_HIGH);
+static DigitalIn din4(mcu::GPIO3, 0, board::DIN_HIGH);
+
+static DigitalIn hvdin1(mcu::GPIO4, 1, board::DIN_LOW);
+static DigitalIn hvdin2(mcu::GPIO5, 1, board::DIN_LOW);
+static DigitalIn hvdin3(mcu::GPIO6, 1, board::DIN_LOW); 
+static DigitalIO hvdin3_pull(mcu::GPIO3, board::DIN_HIGH);
+	
+static DigitalOut odout1(mcu::GPIO9,  1);
+static DigitalOut odout2(mcu::GPIO10, 1);
+static DigitalOut odout3(mcu::GPIO11, 1);
+static DigitalOut odout4(mcu::GPIO12, 1);
+static DigitalOut odout5(mcu::GPIO13, 1);
+static DigitalOut odout6(mcu::GPIO14, 1);
+	
+static PWMout od_pwm(mcu::PWM1);
+
+/**** Public function declarations ****/
+
+#ifdef TESTING
+#endif
+
+#endif /* UDCCD_BOARD_H_ */
--- a/firmware/src/bsp/ain_lpf.cpp
+++ b/firmware/src/bsp/ain_lpf.cpp
@@ -1,52 +0,0 @@
-/**** 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 ****/
--- a/firmware/src/bsp/ain_lpf.h
+++ b/firmware/src/bsp/ain_lpf.h
@@ -1,36 +0,0 @@
-#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_ */
--- a/firmware/src/bsp/board.cpp
+++ b/firmware/src/bsp/board.cpp
@@ -1,108 +0,0 @@
-/**** 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 ****/
-bsp::Board::Board(void)
-{
-	return;
-}
-
-bsp::Board::~Board(void)
-{
-	return;
-}
-
-void bsp::Board::init(boardCfg_t* cfg)
-{
-	// Calculate settings
-	
-	// Controller setup
-	mcu::startupCfg_t mcu_cfg;
-	
-	mcu_cfg.adc_clk = mcu::ADC_DIV64; // 8MHz/64=125kHz
-	mcu_cfg.pwm_clk = mcu::TIM_DIV1;  // 8MHz/1 = 8MHz
-	mcu_cfg.pwm_top = 4000/(uint16_t)cfg->pwm_f_khz;
-	mcu_cfg.od_common_is_pwm = cfg->od_common_is_pwm;
-	
-	mcu::startup(&mcu_cfg);
-	
-	// Analog inputs
-	this->out_voltage.init(mcu::ADC_VOUT);
-	this->out_voltage.mul = 20;
-	this->out_voltage.div = 1;
-	this->out_voltage.offset = 0;
-	
-	this->out_current.init(mcu::ADC_IOUT);
-	this->out_current.mul = 215;
-	this->out_current.div = 22;
-	this->out_current.offset = 0;
-	
-	this->battery_voltage.init(mcu::ADC_VBAT);
-	this->battery_voltage.mul = 20;
-	this->battery_voltage.div = 1;
-	this->battery_voltage.offset = 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 bsp::Board::read(void)
-{
-	// 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();
-	
-	// 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 ****/
-
--- a/firmware/src/bsp/board.h
+++ b/firmware/src/bsp/board.h
@@ -1,75 +0,0 @@
-#ifndef UDCCD_BOARD_H_
-#define UDCCD_BOARD_H_
-
-/**** Includes ****/
-#include <stdint.h>
-
-#include "ain.h"
-#include "ain_lpf.h"
-#include "din.h"
-#include "dout.h"
-#include "pwm_out.h"
-#include "memory.h"
-
-namespace bsp {
-
-/**** Public definitions ****/
-class Board
-{	
-	public:
-	typedef struct {
-		uint8_t pwm_f_khz;
-		uint8_t od_common_is_pwm;
-	} boardCfg_t;
-	
-	Board(void);
-	~Board(void);
-	
-	void init(boardCfg_t* cfg);
-	
-	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;
-	
-	Memory nvmem;
-	
-	void read(void);
-
-	#ifndef TESTING
-	protected:
-	#endif
-};
-
-/**** Public function declarations ****/
-
-#ifdef TESTING
-#endif
-
-} //namespace
-
-#endif /* UDCCD_BOARD_H_ */
--- a/firmware/src/bsp/din.cpp
+++ b/firmware/src/bsp/din.cpp
@@ -1,48 +0,0 @@
-/**** Includes ****/
-#include "../utils/utils.h"
-#include "mcu/mcu_hal.h"
-#include "din.h"
-
-using namespace bsp;
-
-/**** Private definitions ****/
-/**** Private constants ****/
-/**** Private variables ****/
-/**** Private function declarations ****/
-/**** Public function definitions ****/
-bsp::DigitalIn::DigitalIn(void)
-{
-	return;
-}
-
-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)
-{
-	// Read ADC
-	this->last_read = mcu::gpio_read(this->gpio_ch);
-	
-	// 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 ****/
-
--- a/firmware/src/bsp/dout.cpp
+++ b/firmware/src/bsp/dout.cpp
@@ -1,37 +0,0 @@
-/**** Includes ****/
-#include "../utils/utils.h"
-#include "mcu/mcu_hal.h"
-#include "dout.h"
-
-using namespace bsp;
-
-/**** Private definitions ****/
-/**** Private constants ****/
-/**** Private variables ****/
-/**** Private function declarations ****/
-/**** Public function definitions ****/
-bsp::DigitalOut::DigitalOut(void)
-{
-	return;
-}
-
-bsp::DigitalOut::~DigitalOut(void)
-{
-	return;
-}
-
-void bsp::DigitalOut::write(int8_t level)
-{
-	if(this->is_inverted)
-	{
-		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 ****/
-
--- a/firmware/src/bsp/dout.h
+++ b/firmware/src/bsp/dout.h
@@ -1,34 +0,0 @@
-#ifndef DIGITAL_OUT_H_
-#define DIGITAL_OUT_H_
-
-/**** Includes ****/
-#include <stdint.h>
-#include "din.h"
-
-namespace bsp {
-
-/**** Public definitions ****/
-class DigitalOut : public DigitalIn
-{	
-	public:
-	// New or redefined stuff
-	DigitalOut(void);
-	~DigitalOut(void);
-	
-	int8_t last_writen;
-	
-	void write(int8_t level);
-	
-	#ifndef TESTING
-	protected:
-	#endif
-};
-
-/**** Public function declarations ****/
-
-#ifdef TESTING
-#endif
-
-} //namespace
-
-#endif /* DIGITAL_OUT_H_ */
--- a/firmware/src/bsp/mcu/mcu_hal.h
+++ b/firmware/src/bsp/mcu/mcu_hal.h
@@ -1,114 +0,0 @@
-#ifndef MCU_HAL_H_
-#define MCU_HAL_H_
-
-/**** Includes ****/
-#include <stdint.h>
-
-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;
-const uint8_t GPIO_DIN3 = 2;
-const uint8_t GPIO_DIN4 = 3;
-const uint8_t GPIO_HVDIN1 = 4;
-const uint8_t GPIO_HVDIN2 = 5;
-const uint8_t GPIO_HVDIN3 = 6;
-const uint8_t GPIO_HVDIN3_PULL = 7;
-const uint8_t GPIO_OD1 = 8;
-const uint8_t GPIO_OD2 = 9;
-const uint8_t GPIO_OD3 = 10;
-const uint8_t GPIO_OD4 = 11;
-const uint8_t GPIO_OD5 = 12;
-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_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
-const uint8_t ADC_VBAT  = 2;	//Battery voltage
-const uint8_t ADC_IBAT  = 3;	//Battery current
-const uint8_t ADC_AIN2  = 4;	//Potentiometer
-const uint8_t ADC_AIN1  = 5;	//Mode
-const uint8_t ADC_TEMP  = 8;	//MCU temperature
-const uint8_t ADC_IVREF = 14;	//MCU internal reference
-const uint8_t ADC_GND   = 15;	//MCU ground
-
-const uint8_t PWM_OUT = 0;	//DCCD
-const uint8_t PWM_OD  = 1;	//LED
-
-//ADC definitions
-typedef enum {
-	ADC_DIV2 = 0x01,
-	ADC_DIV4 = 0x02,
-	ADC_DIV8 = 0x03,
-	ADC_DIV16 = 0x04,
-	ADC_DIV32 = 0x05,
-	ADC_DIV64 = 0x06,
-	ADC_DIV128 = 0x07
-} adcClkDiv_t;
-
-//Timer definitions
-typedef enum {
-	TIM_DIV1 = 0x01,
-	TIM_DIV8 = 0x02,
-	TIM_DIV64 = 0x03,
-	TIM_DIV256 = 0x04,
-	TIM_DIV1024 = 0x05
-} timerClkDiv_t;
-
-typedef struct {
-	adcClkDiv_t adc_clk;
-	timerClkDiv_t pwm_clk;
-	uint16_t pwm_top;
-	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);
-uint16_t pwm_read(uint8_t ch);
-
-void timer_reset(uint8_t ch);
-uint16_t timer_read(uint8_t ch);
-uint16_t timer_read_top(uint8_t ch);
-
-uint32_t timer_convert_us(uint8_t ch, uint16_t raw);
-uint32_t timer_convert_ms(uint8_t ch, uint16_t raw);
-
-uint8_t eeprom_read8b(uint16_t address);
-uint16_t eeprom_read16b(uint16_t address);
-uint32_t eeprom_read32b(uint16_t address);
-
-void eeprom_write8b(uint16_t address, uint8_t value);
-void eeprom_write16b(uint16_t address, uint16_t value);
-void eeprom_write32b(uint16_t address, uint32_t value);
-
-} //namespace
-
-#endif /* MCU_HAL_H_ */
--- a/firmware/src/bsp/memory.cpp
+++ b/firmware/src/bsp/memory.cpp
@@ -1,55 +0,0 @@
-/**** Includes ****/
-#include "mcu/mcu_hal.h"
-#include "memory.h"
-
-using namespace bsp;
-
-/**** Private definitions ****/
-/**** Private constants ****/
-/**** Private variables ****/
-/**** Private function declarations ****/
-/**** Public function definitions ****/
-bsp::Memory::Memory(void)
-{
-	return;
-}
-
-bsp::Memory::~Memory(void)
-{
-	return;
-}
-
-uint8_t bsp::Memory::read_8b(uint16_t address)
-{
-	return mcu::eeprom_read8b(address);
-}
-
-
-uint16_t bsp::Memory::read_16b(uint16_t address)
-{
-	return mcu::eeprom_read16b(address);
-}
-
-uint32_t bsp::Memory::read_32b(uint16_t address)
-{
-	return mcu::eeprom_read32b(address);
-}
-
-void bsp::Memory::write_8b(uint16_t address, uint8_t value)
-{
-	mcu::eeprom_write8b(address, value);
-}
-
-
-void bsp::Memory::write_16b(uint16_t address, uint16_t value)
-{
-	mcu::eeprom_write16b(address, value);
-}
-
-void bsp::Memory::write_32b(uint16_t address, uint32_t value)
-{
-	mcu::eeprom_write32b(address, value);
-}
-
-/**** Private function definitions ****/
-
--- a/firmware/src/bsp/memory.h
+++ b/firmware/src/bsp/memory.h
@@ -1,36 +0,0 @@
-#ifndef MEMORY_IN_H_
-#define MEMORY_IN_H_
-
-/**** Includes ****/
-#include <stdint.h>
-
-namespace bsp {
-
-/**** Public definitions ****/
-class Memory
-{	
-	public:
-	Memory(void);
-	~Memory(void);
-	
-	uint8_t read_8b(uint16_t address);
-	uint16_t read_16b(uint16_t address);
-	uint32_t read_32b(uint16_t address);
-	
-	void write_8b(uint16_t address, uint8_t value);
-	void write_16b(uint16_t address, uint16_t value);
-	void write_32b(uint16_t address, uint32_t value);
-	
-	#ifndef TESTING
-	protected:
-	#endif
-};
-
-/**** Public function declarations ****/
-
-#ifdef TESTING
-#endif
-
-} //namespace
-
-#endif /* MEMORY_IN_H_ */
--- a/firmware/src/bsp/pwm_out.cpp
+++ b/firmware/src/bsp/pwm_out.cpp
@@ -1,56 +0,0 @@
-/**** 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 ****/
--- a/firmware/src/bsp/pwm_out.h
+++ b/firmware/src/bsp/pwm_out.h
@@ -1,37 +0,0 @@
-#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_ */
--- a/firmware/src/dccd/dccd.cpp
+++ b/firmware/src/dccd/dccd.cpp
@@ -1,432 +0,0 @@
-/**** Includes ****/
-#include "../utils/utils.h"
-#include "dccd.h"
-
-using namespace dccd;
-
-/**** Private definitions ****/
-/**** Private constants ****/
-static const uint16_t def_lock_current = 4500;
-static const uint16_t def_max_hbrake_time = 0;
-static const uint16_t def_btn_force_repeat_time = 300;
-static const uint16_t def_btn_mode_repeat_time = 700;
-static const uint8_t def_button_inputs = 1;
-
-static const uint8_t def_display_brigth = 100;
-static const uint8_t def_display_dimm = 50;
-
-static const uint16_t cv_ref_resistance = 1500;
-static const uint16_t cc_ref_resistance = 2000;
-static const uint16_t cc_min_resistance = 1000;
-
-static const uint8_t bmode_image_open = 0x07;
-static const uint8_t bmode_image_user = 0x1E;
-static const uint8_t bmode_image_lock = 0x38;
-
-static const uint16_t display_keep_bmode = 2000;
-static const uint16_t display_keep_userf = 1000;
-
-static const uint8_t user_force_step = 10;
-
-static const uint8_t def_btn_force = 0;
-static const uint8_t def_brake_mode = 0;
-
-static const uint16_t def_chasis_inp_debounce = 100;
-static const uint16_t def_user_inp_debounce = 20;
-
-static const uint16_t mem_addr_inp_mode = 0;
-static const uint16_t mem_addr_force = 1;
-static const uint16_t mem_addr_bmode = 2;
-static const uint16_t mem_addr_dsp_brigth = 3;
-static const uint16_t mem_addr_dsp_dimm = 4;
-static const uint16_t mem_addr_lock_current = 5;
-static const uint16_t mem_addr_hbrake_time = 7;
-
-/**** 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 ****/
-dccd::DccdApp::DccdApp(void)
-{
-	return;
-}
-
-dccd::DccdApp::~DccdApp(void)
-{
-	return;
-}
-
-void dccd::DccdApp::init(DccdHw* dccd_hw)
-{
-	this->hardware = dccd_hw;
-	
-	#define OVERRIDEDEDBNC
-	#ifdef OVERRIDEDEDBNC
-	this->hardware->btn_mode.dbnc_lim = def_user_inp_debounce;
-	this->hardware->btn_up.dbnc_lim = def_user_inp_debounce;
-	this->hardware->btn_down.dbnc_lim = def_user_inp_debounce;
-	this->hardware->handbrake.dbnc_lim = def_chasis_inp_debounce;
-	this->hardware->brakes.dbnc_lim = def_chasis_inp_debounce;
-	this->hardware->dimm.dbnc_lim = def_chasis_inp_debounce;
-	#endif
-	
-	// Load saved config from memory
-	this->loadMemCfg();
-	
-	this->btn_force_repeat_time = def_btn_force_repeat_time;
-	this->btn_mode_repeat_time = def_btn_mode_repeat_time;
-	this->pot_force = 0;
-	
-	this->hardware->read();
-	this->hardware->dimm.force_read();
-
-	this->hardware->outreg.write_voltage(0);
-	this->hardware->outreg.write_current(0);
-	this->hardware->outreg.write_on(1);
-	
-	this->hardware->display.write(0x01);
-	if(this->hardware->dimm.state) this->hardware->display.write_backlight(this->display_dimm);
-	else this->hardware->display.write_backlight(this->display_brigth);
-	
-	this->hardware->write();
-}
-
-void dccd::DccdApp::process(void)
-{
-	// Update all inputs
-	this->hardware->read();
-	
-	uint8_t is_new_mode = 0;
-	uint8_t is_new_btn_force = 0;
-	
-	// Process mode button
-	if((this->hardware->btn_mode.state==1)&&((this->hardware->btn_mode.is_new)||(this->hardware->btn_mode.time_read() >= this->btn_mode_repeat_time)))
-	{
-		this->hardware->btn_mode.time_reset();
-		this->hardware->btn_mode.is_new = 0;
-		// Change mode
-		switch(this->brake_mode)
-		{
-			case 0:
-			this->brake_mode = 1;
-			break;
-			
-			case 1:
-			this->brake_mode = 2;
-			break;
-			
-			default:
-			this->brake_mode = 0;
-			break;
-		}
-		is_new_mode = 1;
-		this->hardware->board_hw.nvmem.write_8b(mem_addr_bmode, this->brake_mode);
-	};
-	
-	// Process user force inputs
-	if((this->hardware->btn_up.state==1)&&((this->hardware->btn_up.is_new)||(this->hardware->btn_up.time_read() >= this->btn_force_repeat_time)))
-	{
-		this->hardware->btn_up.time_reset();
-		this->hardware->btn_up.is_new = 0;
-		// Increase user force
-		this->btn_force += user_force_step;
-		if(this->btn_force > 100) this->btn_force = 100;
-		is_new_btn_force = 1;
-	};
-	
-	if((this->hardware->btn_down.state==1)&&((this->hardware->btn_down.is_new)||(this->hardware->btn_down.time_read() >= this->btn_force_repeat_time)))
-	{
-		this->hardware->btn_down.time_reset();
-		this->hardware->btn_down.is_new = 0;
-		// Decrease user force
-		this->btn_force -= user_force_step;
-		if(this->btn_force > 100) this->btn_force = 0;
-		is_new_btn_force = 1;
-	};
-	
-	if(is_new_btn_force)
-	{
-		this->hardware->board_hw.nvmem.write_8b(mem_addr_force, this->btn_force);
-	};
-	
-	this->pot_force = this->hardware->pot.last_percent;
-	
-	// Determine user force
-	int8_t user_force;
-	if(this->button_inputs) user_force = (int8_t)this->btn_force;
-	else user_force = (int8_t)this->pot_force;
-	
-	// Determine next settable force
-	int8_t next_force;
-	
-	uint8_t hbrake_timeout = 0;
-	if((this->max_hbrake_time!=0)&&(this->hardware->handbrake.time_read() >= this->max_hbrake_time))
-	{
-		hbrake_timeout = 1;
-	};
-	
-	if((this->hardware->handbrake.state == 1)&&(hbrake_timeout==0))
-	{
-		// Handbrake override
-		next_force = -1;
-	}
-	else if(this->hardware->brakes.state == 1)
-	{
-		// Brakes override
-		switch(this->brake_mode)
-		{
-			case 0:
-			next_force = -1;
-			break;
-			
-			case 1:
-			next_force = user_force;
-			break;
-			
-			case 2:
-			next_force = 100;
-			break;
-			
-			default:
-			next_force = -1;
-			this->brake_mode = 0;
-			break;
-		}
-	}
-	else
-	{
-		// User force
-		next_force = user_force;
-	}
-	
-	// Apply next force
-	if(next_force < 0)
-	{
-		// HiZ
-		this->hardware->outreg.write_voltage(0);
-		this->hardware->outreg.write_current(0);
-		this->hardware->outreg.write_on(0);
-		// For display
-		next_force = 0;
-	}
-	else if(next_force == 0)
-	{
-		// Open
-		this->hardware->outreg.write_voltage(0);
-		this->hardware->outreg.write_current(0);
-		this->hardware->outreg.write_on(1);
-	}
-	else
-	{
-		// Calculate current and voltage settings
-		this->hardware->outreg.write_current(util::percent_of((uint8_t)next_force, this->lock_current));
-		uint16_t ref_resistance = cv_ref_resistance;
-		if(this->hardware->outreg.cc_mode_en) ref_resistance = cc_ref_resistance;
-		this->hardware->outreg.write_voltage(util::sat_mul_kilo(this->hardware->outreg.read_current(), ref_resistance));
-		this->hardware->outreg.write_on(1);
-	}
-	
-	// Display image
-	if(is_new_mode)
-	{
-		uint8_t bmode_image;
-		switch(this->brake_mode)
-		{
-			case 0:
-			bmode_image = bmode_image_open;
-			break;
-			
-			case 1:
-			bmode_image = bmode_image_user;
-			break;
-			
-			case 2:
-			bmode_image = bmode_image_lock;
-			break;
-			
-			default:
-			bmode_image = bmode_image_open;
-			this->brake_mode = 0;
-			break;
-		}
-		this->hardware->display.write(bmode_image, display_keep_bmode, display_keep_bmode, 1);
-		is_new_mode = 0;
-	}
-	else if((is_new_btn_force)&&(this->button_inputs))
-	{
-		this->hardware->display.write(img_gen_dot10(this->btn_force), display_keep_userf, display_keep_userf, 1);
-		is_new_btn_force = 0;
-	}
-	else if(this->hardware->display.is_cycle_end())
-	{
-		this->hardware->display.write(img_gen_dot10((uint8_t)next_force));	
-	};
-	
-	// Display backlight
-	if(this->hardware->dimm.is_new)
-	{
-		this->hardware->dimm.is_new = 0;
-		if(this->hardware->dimm.state) this->hardware->display.write_backlight(this->display_dimm);
-		else this->hardware->display.write_backlight(this->display_brigth);
-	};
-	
-	// Execute outputs
-	this->hardware->write();
-}
-
-uint8_t dccd::DccdApp::loadMemCfg(void)
-{
-	// Load saved config from memory
-	uint8_t t1;
-	uint16_t t2;
-	uint8_t def_applied = 0;
-	
-	t1 = this->hardware->board_hw.nvmem.read_8b(mem_addr_inp_mode);
-	if(t1 > 1){this->button_inputs = def_button_inputs; def_applied=1; }
-	else this->button_inputs = t1;
-	
-	t1 = this->hardware->board_hw.nvmem.read_8b(mem_addr_force);
-	if(t1 > 100){this->btn_force = def_btn_force; def_applied=1; }
-	else this->btn_force = t1;
-	
-	t1 = this->hardware->board_hw.nvmem.read_8b(mem_addr_bmode);
-	if(t1 > 2){this->brake_mode = def_brake_mode; def_applied=1; }
-	else this->brake_mode = t1;
-	
-	t1 = this->hardware->board_hw.nvmem.read_8b(mem_addr_dsp_brigth);
-	if((t1 > 100)||(t1 == 0)){this->display_brigth = def_brake_mode; def_applied=1; }
-	else this->display_brigth = t1;
-	
-	t1 = this->hardware->board_hw.nvmem.read_8b(mem_addr_dsp_dimm);
-	if((t1 > 100)||(t1 == 0)){this->display_dimm = def_brake_mode; def_applied=1; }
-	else this->display_dimm = t1;
-	
-	t2 = this->hardware->board_hw.nvmem.read_16b(mem_addr_lock_current);
-	if((t2 > 5000)||(t2 < 1000)){this->lock_current = def_lock_current; def_applied=1; }
-	else this->lock_current = t2;
-	
-	t2 = this->hardware->board_hw.nvmem.read_16b(mem_addr_hbrake_time);
-	if((t2 > 30000)||(t2 == 0)){this->max_hbrake_time = def_max_hbrake_time; def_applied=1; }
-	else this->max_hbrake_time = t2;
-	
-	return def_applied;
-}
-
-void dccd::DccdApp::saveMemCfg(void)
-{
-	// Save config to memory
-	
-	this->hardware->board_hw.nvmem.write_8b(mem_addr_inp_mode, this->button_inputs);
-	
-	this->hardware->board_hw.nvmem.write_8b(mem_addr_force, this->btn_force);
-	
-	this->hardware->board_hw.nvmem.write_8b(mem_addr_bmode, this->brake_mode);
-	
-	this->hardware->board_hw.nvmem.write_8b(mem_addr_dsp_brigth, this->display_brigth);
-	
-	this->hardware->board_hw.nvmem.write_8b(mem_addr_dsp_dimm, this->display_dimm);
-	
-	this->hardware->board_hw.nvmem.write_16b(mem_addr_lock_current, this->lock_current);
-	
-	this->hardware->board_hw.nvmem.write_16b(mem_addr_hbrake_time, this->max_hbrake_time);
-}
-
-/**** 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;
-		
-		case 96 ... 100:
-		return 0x20;
-		
-		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;
-		
-		case 91 ... 100:
-		return 0x20;
-		
-		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;
-		
-		case 91 ... 100:
-		return 0x3F;
-		
-		default:
-		return 0x3F;
-	}
-}
--- a/firmware/src/dccd/dccd.h
+++ b/firmware/src/dccd/dccd.h
@@ -1,48 +0,0 @@
-#ifndef DCCD_APP_H_
-#define DCCD_APP_H_
-
-/**** Includes ****/
-#include <stdint.h>
-#include "dccd_hw.h"
-
-namespace dccd {
-
-/**** Public definitions ****/
-class DccdApp
-{
-	public:
-	DccdApp(void);
-	~DccdApp(void);
-	
-	void init(DccdHw* dccd_hw);
-	void process(void);
-	
-	uint16_t lock_current;
-	uint16_t max_hbrake_time;
-	uint16_t btn_force_repeat_time;
-	uint16_t btn_mode_repeat_time;
-	uint8_t button_inputs;
-	uint8_t display_brigth;
-	uint8_t display_dimm;
-	
-	uint8_t btn_force;
-	uint8_t pot_force;
-	uint8_t brake_mode;
-	
-	uint8_t loadMemCfg(void);
-	void saveMemCfg(void);
-	
-	#ifdef TESTING
-	protected:
-	#endif
-	DccdHw* hardware;
-};
-
-/**** Public function declarations ****/
-
-#ifdef TESTING
-#endif
-
-} //namespace
-
-#endif /* DCCD_APP_H_ */
--- a/firmware/src/dccd/dccd_hw.cpp
+++ b/firmware/src/dccd/dccd_hw.cpp
@@ -1,190 +0,0 @@
-/**** Includes ****/
-#include "../utils/utils.h"
-#include "dccd_hw.h"
-
-using namespace dccd;
-
-/**** Private definitions ****/
-/**** Private constants ****/
-static const uint8_t def_dbnc_time = 10;
-
-static const uint16_t def_pot_dead_bot = 500;
-static const uint16_t def_pot_dead_top = 4500;
-
-static const uint8_t def_cc_mode_en = 1;
-
-static const uint16_t def_cnter_us = 900;
-
-static const uint16_t def_out_voltage_under_treshold = 0;
-static const uint16_t def_out_voltage_over_treshold = 9000;
-static const uint16_t def_out_voltage_hold_time = 1000;
-static const uint16_t def_out_voltage_cooldown_time = 0;
-
-static const uint16_t def_out_current_under_treshold = 0;
-static const uint16_t def_out_current_over_treshold = 6000;
-static const uint16_t def_out_current_hold_time = 200;
-static const uint16_t def_out_current_cooldown_time = 1000;
-
-static const uint16_t def_battery_voltage_under_treshold = 9000;
-static const uint16_t def_battery_voltage_over_treshold = 18000;
-static const uint16_t def_battery_voltage_hold_time = 1000;
-static const uint16_t def_battery_voltage_cooldown_time = 0;
-
-static const uint16_t def_battery_current_under_treshold = 0;
-static const uint16_t def_battery_current_over_treshold = 8000;
-static const uint16_t def_battery_current_hold_time = 200;
-static const uint16_t def_battery_current_cooldown_time = 1000;
-
-static const uint16_t def_inital_bat_voltage = 12000;
-
-/**** Private variables ****/
-/**** Private function declarations ****/
-/**** Public function definitions ****/
-dccd::DccdHw::DccdHw(void)
-{
-	return;
-}
-
-dccd::DccdHw::~DccdHw(void)
-{
-	return;
-}
-
-void dccd::DccdHw::init(dccdHwCfg_t* cfg)
-{	
-	// Apply config
-	bsp::Board::boardCfg_t board_cfg;
-	board_cfg.pwm_f_khz = cfg->pwm_f_khz;
-	board_cfg.od_common_is_pwm = 1;
-	this->board_hw.init(&board_cfg);
-	
-	this->counter.init(0xFFFF, cfg->counter_step_us);
-	this->counter.disabled = 0;
-	
-	this->out_voltage.init(&(this->board_hw.out_voltage), &(this->counter));
-	this->out_voltage.under_treshold = def_out_voltage_under_treshold;
-	this->out_voltage.over_treshold = def_out_voltage_over_treshold;
-	this->out_voltage.hold_time = def_out_voltage_hold_time;
-	this->out_voltage.cooldown_time = def_out_voltage_cooldown_time;
-	this->out_voltage.update_ain = 0;
-	this->out_voltage.auto_reset = 1;
-	
-	this->out_current.init(&(this->board_hw.out_current), &(this->counter));
-	this->out_current.under_treshold = def_out_current_under_treshold;
-	this->out_current.over_treshold = def_out_current_over_treshold;
-	this->out_current.hold_time = def_out_current_hold_time;
-	this->out_current.cooldown_time = def_out_current_cooldown_time;
-	this->out_current.update_ain = 0;
-	this->out_current.auto_reset = 1;
-	
-	this->battery_voltage.init(&(this->board_hw.battery_voltage), &(this->counter));
-	this->battery_voltage.under_treshold = def_battery_voltage_under_treshold;
-	this->battery_voltage.over_treshold = def_battery_voltage_over_treshold;
-	this->battery_voltage.hold_time = def_battery_voltage_hold_time;
-	this->battery_voltage.cooldown_time = def_battery_voltage_cooldown_time;
-	this->battery_voltage.update_ain = 0;
-	this->battery_voltage.auto_reset = 1;
-	this->battery_voltage.last_read = def_inital_bat_voltage;
-	
-	this->battery_current.init(&(this->board_hw.battery_current), &(this->counter));
-	this->battery_current.under_treshold = def_battery_current_under_treshold;
-	this->battery_current.over_treshold = def_battery_current_over_treshold;
-	this->battery_current.hold_time = def_battery_current_hold_time;
-	this->battery_current.cooldown_time = def_battery_current_cooldown_time;
-	this->battery_current.update_ain = 0;
-	this->battery_current.auto_reset = 1;
-	
-	this->btn_up.init(&(this->board_hw.din4), 0, &(this->counter), def_dbnc_time);
-	this->btn_up.update_din = 0;
-	
-	this->btn_down.init(&(this->board_hw.din3), 0, &(this->counter), def_dbnc_time);
-	this->btn_down.update_din = 0;
-	
-	this->btn_mode.init(&(this->board_hw.din1), 0, &(this->counter), def_dbnc_time);
-	this->btn_mode.update_din = 0;
-	
-	this->handbrake.init(&(this->board_hw.hvdin3), 0, &(this->counter), def_dbnc_time);
-	this->handbrake.update_din = 0;
-	
-	this->brakes.init(&(this->board_hw.hvdin2), 1, &(this->counter), def_dbnc_time);
-	this->brakes.update_din = 0;
-	
-	this->dimm.init(&(this->board_hw.hvdin1), 1, &(this->counter), def_dbnc_time);
-	this->dimm.update_din = 0;
-	
-	this->pot.init(&(this->board_hw.ain2), def_pot_dead_bot, def_pot_dead_top);
-	this->pot.update_ain = 0;
-	
-	hw::OutReg::outRegCfg_t outreg_cfg;
-	outreg_cfg.pwm_high = &this->board_hw.out_pwm;
-	outreg_cfg.dout_low = &this->board_hw.out_low;
-	outreg_cfg.ubat = &this->board_hw.battery_voltage;
-	outreg_cfg.uout = &this->board_hw.out_voltage;
-	outreg_cfg.iout = &this->board_hw.out_current;
-	this->outreg.init(&outreg_cfg);
-	this->outreg.cc_mode_en = def_cc_mode_en;
-	this->outreg.update_ain = 0;
-	
-	hw::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->outreg.write_voltage(0);
-	this->outreg.write_current(0);
-	this->outreg.write_on(0);
-	this->outreg.write_lock(0);
-	this->outreg.process();
-	
-	this->display.write_backlight(100);
-	this->display.write(0x00);
-}
-
-void dccd::DccdHw::read(void)
-{
-	// Update low level inputs
-	this->board_hw.read();
-	
-	this->counter.increment();
-	
-	this->out_voltage.process();
-	this->out_current.process();
-	this->battery_voltage.process();
-	this->battery_current.process();
-	
-	this->btn_up.process();
-	this->btn_down.process();
-	this->btn_mode.process();
-	this->handbrake.process();
-	this->brakes.process();
-	this->dimm.process();
-	
-	this->pot.read();
-}
-
-void dccd::DccdHw::write(void)
-{
-	this->display.process();
-	this->outreg.process();
-}
-
-/**** Private function definitions ***/
--- a/firmware/src/dccd/dccd_hw.h
+++ b/firmware/src/dccd/dccd_hw.h
@@ -1,69 +0,0 @@
-#ifndef DCCD_HW_H_
-#define DCCD_HW_H_
-
-/**** Includes ****/
-#include <stdint.h>
-#include "../bsp/board.h"
-#include "../utils/vcounter.h"
-#include "../hw/button.h"
-#include "../hw/led_display.h"
-#include "../hw/potentiometer.h"
-#include "../hw/out_driver.h"
-#include "../hw/safe_ain.h"
-#include "../hw/out_reg.h"
-
-namespace dccd {
-
-/**** Public definitions ****/
-class DccdHw
-{
-	public:
-	typedef struct {
-		uint8_t pwm_f_khz;
-		uint8_t handbrake_pull_up;
-		uint8_t speed_hall;
-		uint16_t counter_step_us;
-	} dccdHwCfg_t;
-		
-	DccdHw(void);
-	~DccdHw(void);
-	
-	void init(dccdHwCfg_t* cfg);
-	
-	// Inputs
-	hw::SafeAin out_voltage;
-	hw::SafeAin out_current;
-	hw::SafeAin battery_voltage;
-	hw::SafeAin battery_current;
-	
-	hw::Button btn_up;
-	hw::Button btn_down;
-	hw::Button btn_mode;
-	hw::Button handbrake;
-	hw::Button brakes;
-	hw::Button dimm;
-	
-	hw::Potentiometer pot;
-	
-	// Outputs
-	hw::LedDisplay display;
-	hw::OutReg outreg;
-	
-	void read(void);
-	void write(void);
-	
-	#ifdef TESTING
-	protected:
-	#endif
-	bsp::Board board_hw;
-	util::VCounter counter;
-};
-
-/**** Public function declarations ****/
-
-#ifdef TESTING
-#endif
-
-} //namespace
-
-#endif /* DCCD_HW_H_ */
--- a/firmware/src/hw/button.cpp
+++ b/firmware/src/hw/button.cpp
@@ -8,10 +8,23 @@ using namespace hw;
 /**** Private constants ****/
 /**** Private variables ****/
 /**** Private function declarations ****/
+
 /**** Public function definitions ****/
-hw::Button::Button(void)
+hw::Button::Button(board::DigitalIn* din_ch, uint8_t act_lvl, uint8_t dbnc_lim, uint8_t init_state)
 {
-	return;
+	this->din_ch = din_ch;
+	
+	if(act_lvl) this->act_lvl = board::DIN_HIGH;
+	else this->act_lvl = board::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;
 }

 hw::Button::~Button(void)
@@ -19,67 +32,41 @@ hw::Button::~Button(void)
 	return;
 }

-void hw::Button::init(bsp::DigitalIn* din_ch, uint8_t act_lvl, util::VCounter* timer, uint16_t dbnc_lim)
+uint8_t hw::Button::read(void)
 {
-	this->din_ch = din_ch;
-	this->timer = timer;
+	// Read din level
+	uint8_t lvl = this->din_ch->read();
 	
-	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);
 	
 	// Determine next state
 	uint8_t next_state = BUTTON_OFF;
 	if(lvl==this->act_lvl) next_state = BUTTON_ON;
 	
 	// Advance debounce sample counter
-	uint16_t ts_now = this->timer->read();
+	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;
 	
-	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_read(void)
 {
-	// Read din
-	if(this->update_din) this->din_ch->read();
-	
-	// Get last read level
-	uint8_t lvl = this->din_ch->last_read;
+	// Read din level
+	uint8_t lvl = this->din_ch->read();
 	
 	// Cancels active debounce
-	this->dbnc_ts = 0;
+	this->dbnc_cnter = 0;
 	
 	// Determine next state
 	uint8_t next_state = BUTTON_OFF;
@@ -87,30 +74,12 @@ uint8_t hw::Button::force_read(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 ****/
--- a/firmware/src/hw/button.h
+++ b/firmware/src/hw/button.h
@@ -1,49 +1,41 @@
-#ifndef BUTTONS_H_
-#define BUTTONS_H_
+#ifndef BUTTON_H_
+#define BUTTON_H_

 /**** Includes ****/
 #include <stdint.h>
-#include "../utils/vcounter.h"
-#include "../bsp/board.h"
+#include "../board/din.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:
+	board::DigitalIn* din_ch;
+	uint8_t act_lvl;
+	uint8_t dbnc_cnter;
+	
 	public:
-	Button(void);
+	Button(board::DigitalIn* din_ch, uint8_t act_lvl, uint8_t dbnc_lim, uint8_t init_state);
 	~Button(void);
 	
 	uint8_t state;
-	uint16_t dbnc_lim;
+	uint16_t time;
+	uint8_t dbnc_lim;
 	uint8_t is_new;
-	uint8_t update_din;
 	
-	void init(bsp::DigitalIn* din_ch, uint8_t act_lvl, util::VCounter* timer, uint16_t dbnc_lim);
-	uint8_t process(void);
+	uint8_t read(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 /* BUTTONS_H_ */
+#endif /* BUTTON_H_ */
--- a/firmware/src/hw/cc_driver.cpp
+++ b/firmware/src/hw/cc_driver.cpp
@@ -0,0 +1,38 @@
+/**** Includes ****/
+#include "../utils/utils.h"
+#include "../utils/interpolate.h"
+#include "potentiometer.h"
+
+using namespace hw;
+
+/**** Private definitions ****/
+/**** Private constants ****/
+/**** Private variables ****/
+/**** Private function declarations ****/
+
+/**** Public function definitions ****/
+hw::Potentiometer::Potentiometer(board::AnalogIn* ain_ch, uint16_t low_deadzone, uint16_t high_deadzone)
+{
+	this->ain_ch = ain_ch;
+	this->low_deadzone = low_deadzone;
+	this->high_deadzone = high_deadzone;
+	this->percent = 0;
+}
+
+hw::Potentiometer::~Potentiometer(void)
+{
+	return;
+}
+
+uint8_t hw::Potentiometer::read(void)
+{
+	// Update input
+	this->ain_ch->read();
+	
+	// Calculate percent
+	this->percent = util::interpolate(this->ain_ch->last_read, this->low_deadzone, this->high_deadzone, 0, 100);
+	
+	return this->percent;
+}
+
+/**** Private function definitions ****/
--- a/firmware/src/hw/cc_driver.h
+++ b/firmware/src/hw/cc_driver.h
@@ -0,0 +1,33 @@
+#ifndef CONST_CURRENT_DRIVER_H_
+#define CONST_CURRENT_DRIVER_H_
+
+/**** Includes ****/
+#include <stdint.h>
+#include "../board/ain.h"
+#include "../board/halfbridge.h"
+
+namespace hw {
+
+/**** Public definitions ****/
+
+class CCdriver
+{
+	protected:
+	board::AnalogIn* sup_voltage;
+	board::AnalogIn* sup_current;
+	board::AnalogIn* ain_ch;
+	board::AnalogIn* ain_ch;
+	
+	public:
+	CCdriver(board::AnalogIn* ain_ch);
+	~CCdriver(void);
+};
+
+/**** Public function declarations ****/
+
+#ifdef TESTING
+#endif
+
+} //namespace
+
+#endif /* CONST_CURRENT_DRIVER_H_ */
--- a/firmware/src/hw/display_led.cpp
+++ b/firmware/src/hw/display_led.cpp
@@ -0,0 +1,180 @@
+/**** 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(board::DigitalOut* led0, board::DigitalOut* led1, board::DigitalOut* led2, board::DigitalOut* led3, board::DigitalOut* led4, board::DigitalOut* led5, board::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);
+}
+
+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);
+}
+
+/**** 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;
+	}
+}
--- a/firmware/src/hw/display_led.h
+++ b/firmware/src/hw/display_led.h
@@ -0,0 +1,47 @@
+#ifndef DISPLAY_LED_H_
+#define DISPLAY_LED_H_
+
+/**** Includes ****/
+#include <stdint.h>
+#include "../board/dout.h"
+#include "../board/pwm.h"
+
+namespace hw {
+
+/**** Public definitions ****/
+
+class DisplayLed
+{
+	protected:
+	board::DigitalOut* led0;
+	board::DigitalOut* led1;
+	board::DigitalOut* led2;
+	board::DigitalOut* led3;
+	board::DigitalOut* led4;
+	board::DigitalOut* led5;
+	board::PWMout* common;
+	
+	public:
+	typedef enum {
+		LED_DSP_DOT20,
+		LED_DSP_DOT10,
+		LED_DSP_BAR
+	} style_t;
+	
+	DisplayLed(board::DigitalOut* led0, board::DigitalOut* led1, board::DigitalOut* led2, board::DigitalOut* led3, board::DigitalOut* led4, board::DigitalOut* led5, board::PWMout* common);
+	~DisplayLed(void);
+	
+	void show_percent(uint8_t percent, style_t style);
+	void write(uint8_t image);
+	
+	void set_brigthness(uint8_t percent);
+};
+
+/**** Public function declarations ****/
+
+#ifdef TESTING
+#endif
+
+} //namespace
+
+#endif /* DISPLAY_LED_H_ */
--- a/firmware/src/hw/led_display.cpp
+++ b/firmware/src/hw/led_display.cpp
@@ -1,160 +0,0 @@
-/**** 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->image = 0x00;
-	
-	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;
-	this->image = image;
-	
-	// Set initial state
-	this->force(this->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;
-	this->image = image;
-	
-	// Set initial state
-	if(this->on_time > 0) this->force(this->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 ***/
--- a/firmware/src/hw/led_display.h
+++ b/firmware/src/hw/led_display.h
@@ -1,66 +0,0 @@
-#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_ */
--- a/firmware/src/hw/out_driver.cpp
+++ b/firmware/src/hw/out_driver.cpp
@@ -1,105 +0,0 @@
-/**** 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 ****/
--- a/firmware/src/hw/out_driver.h
+++ b/firmware/src/hw/out_driver.h
@@ -1,45 +0,0 @@
-#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_ */
--- a/firmware/src/hw/out_reg.cpp
+++ b/firmware/src/hw/out_reg.cpp
@@ -1,139 +0,0 @@
-/**** Includes ****/
-#include "../utils/utils.h"
-#include "out_reg.h"
-
-using namespace bsp;
-
-/**** Private definitions ****/
-/**** Private constants ****/
-/**** Private variables ****/
-/**** Private function declarations ****/
-
-/**** Public function definitions ****/
-hw::OutReg::OutReg(void)
-{
-	return;
-}
-
-hw::OutReg::~OutReg(void)
-{
-	return;
-}
-
-void hw::OutReg::init(outRegCfg_t* cfg)
-{
-	this->pwm_high = cfg->pwm_high;
-	this->dout_low = cfg->dout_low;
-	this->ubat = cfg->ubat;
-	this->uout = cfg->uout;
-	this->iout = cfg->iout;
-	
-	this->voltage = 0;
-	this->current = 0;
-	this->out_on = 0;
-	this->lock = 0;
-	
-	this->cc_mode_en = 0;
-	this->update_ain = 0;
-	this->cc_tolerance = 75;
-}
-
-void hw::OutReg::write_voltage(uint16_t voltage)
-{
-	this->voltage = voltage;
-}
-
-void hw::OutReg::write_current(uint16_t current)
-{
-	this->current = current;
-	this->current_bot = util::sat_subtract(current, this->cc_tolerance);
-	this->current_top = util::sat_add(current, this->cc_tolerance);
-}
-
-void hw::OutReg::write_on(uint8_t state)
-{
-	this->out_on = state;
-}
-
-void hw::OutReg::write_lock(uint8_t state)
-{
-	this->lock = state;
-}
-
-uint16_t hw::OutReg::read_voltage(void)
-{
-	return this->voltage;
-}
-
-uint16_t hw::OutReg::read_current(void)
-{
-	return this->current;
-}
-
-void hw::OutReg::process(void)
-{
-	// Update analog input
-	if(this->update_ain)
-	{
-		this->ubat->read();
-		this->uout->read();
-		this->iout->read();
-	};
-	
-	// Check if turned off
-	if((out_on == 0)||(this->lock != 0))
-	{
-		
-		this->pwm_high->write((uint16_t)0);
-		this->dout_low->write(0);
-		return;
-	}
-	else if(this->dout_low->last_writen == 0)
-	{
-		this->dout_low->write(1);
-	};
-
-	// Calculate next duty cycle setting
-	uint16_t next_duty = this->pwm_high->get_set_duty();
-	
-	if((this->voltage==0)||(this->current==0))
-	{
-		// Off but not HiZ
-		next_duty = 0;
-	}
-	else if((this->cc_mode_en)&&(this->iout->last_read > this->current_bot))
-	{
-		// Constant current mode - Change voltage to be within current limit
-		if(util::is_in_range(this->iout->last_read, this->current_bot, this->current_top)==0)
-		{
-			// Current outside of tolerance. Recalculate duty cycle.
-			uint32_t temp = (uint32_t)this->pwm_high->get_set_duty() * (uint32_t)this->current;
-			temp /= this->iout->last_read;
-			next_duty = util::sat_cast(temp);
-		};
-	}
-	else
-	{
-		// Constant voltage mode
-		next_duty = util::sat_ratio(this->voltage, this->ubat->last_read);
-	}
-	
-	this->pwm_high->write(next_duty);
-	
-	return;
-}
-
-void hw::OutReg::force_off(void)
-{
-	// Turn off output - HiZ
-	this->pwm_high->write((uint16_t)0);
-	this->dout_low->write(0);
-	
-	// Update targets
-	this->voltage = 0;
-	this->current = 0;
-	this->out_on = 0;
-	this->lock = 1;
-}
-
-/**** Private function definitions ****/
--- a/firmware/src/hw/out_reg.h
+++ b/firmware/src/hw/out_reg.h
@@ -1,66 +0,0 @@
-#ifndef OUTPUT_REGULATOR_H_
-#define OUTPUT_REGULATOR_H_
-
-/**** Includes ****/
-#include <stdint.h>
-#include "../bsp/board.h"
-
-namespace hw {
-
-/**** Public definitions ****/
-
-class OutReg
-{
-	public:
-	typedef struct {
-		bsp::PwmOut* pwm_high;
-		bsp::DigitalOut* dout_low;
-		bsp::AnalogIn* ubat;
-		bsp::AnalogIn* uout;
-		bsp::AnalogIn* iout;
-	} outRegCfg_t;
-	
-	OutReg(void);
-	~OutReg(void);
-	
-	void init(outRegCfg_t* cfg);
-	
-	uint8_t cc_mode_en;
-	uint8_t update_ain;
-	uint16_t cc_tolerance;
-	
-	void write_voltage(uint16_t voltage);
-	void write_current(uint16_t current);
-	void write_on(uint8_t state);
-	void write_lock(uint8_t state);
-	
-	uint16_t read_voltage(void);
-	uint16_t read_current(void);
-	
-	void process(void);
-	void force_off(void);
-	
-	#ifndef TESTING
-	protected:
-	#endif
-	bsp::PwmOut* pwm_high;
-	bsp::DigitalOut* dout_low;
-	bsp::AnalogIn* ubat;
-	bsp::AnalogIn* uout;
-	bsp::AnalogIn* iout;
-	uint16_t voltage;
-	uint16_t current;
-	uint16_t current_top;
-	uint16_t current_bot;
-	uint8_t out_on;
-	uint8_t lock;
-};
-
-/**** Public function declarations ****/
-
-#ifdef TESTING
-#endif
-
-} //namespace
-
-#endif /* OUTPUT_REGULATOR_H_ */
--- a/firmware/src/hw/potentiometer.cpp
+++ b/firmware/src/hw/potentiometer.cpp
@@ -11,9 +11,12 @@ using namespace hw;
 /**** Private function declarations ****/

 /**** Public function definitions ****/
-hw::Potentiometer::Potentiometer(void)
+hw::Potentiometer::Potentiometer(board::AnalogIn* ain_ch, uint16_t low_deadzone, uint16_t high_deadzone)
 {
-	return;
+	this->ain_ch = ain_ch;
+	this->low_deadzone = low_deadzone;
+	this->high_deadzone = high_deadzone;
+	this->percent = 0;
 }

 hw::Potentiometer::~Potentiometer(void)
@@ -21,26 +24,15 @@ hw::Potentiometer::~Potentiometer(void)
 	return;
 }

-void hw::Potentiometer::init(bsp::AnalogIn* ain_ch, uint16_t low_deadzone, uint16_t high_deadzone)
-{
-	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();
+	// Update input
+	this->ain_ch->read();
 	
 	// 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);
+	this->percent = util::interpolate(this->ain_ch->last_read, this->low_deadzone, this->high_deadzone, 0, 100);
 	
-	return this->last_percent;
+	return this->percent;
 }

 /**** Private function definitions ****/
--- a/firmware/src/hw/potentiometer.h
+++ b/firmware/src/hw/potentiometer.h
@@ -3,31 +3,26 @@

 /**** Includes ****/
 #include <stdint.h>
-#include "../bsp/board.h"
+#include "../board/ain.h"

 namespace hw {

 /**** Public definitions ****/

 class Potentiometer
-{	
-	public:
-	Potentiometer(void);
-	~Potentiometer(void);
+{
+	protected:
+	board::AnalogIn* ain_ch;
 	
-	void init(bsp::AnalogIn* ain_ch, uint16_t low_deadzone, uint16_t high_deadzone);
+	public:
+	Potentiometer(board::AnalogIn* ain_ch, uint16_t low_deadzone, uint16_t high_deadzone);
+	~Potentiometer(void);
 	
 	uint16_t low_deadzone;
 	uint16_t high_deadzone;
-	uint8_t last_percent;
-	uint8_t update_ain;
+	uint8_t percent;
 	
 	uint8_t read(void);
-	
-	#ifndef TESTING
-	protected:
-	#endif
-	bsp::AnalogIn* ain_ch;
 };

 /**** Public function declarations ****/
--- a/firmware/src/hw/safe_ain.cpp
+++ b/firmware/src/hw/safe_ain.cpp
@@ -1,87 +0,0 @@
-/**** Includes ****/
-#include "../utils/utils.h"
-#include "safe_ain.h"
-
-using namespace hw;
-
-/**** Private definitions ****/
-/**** Private constants ****/
-/**** Private variables ****/
-/**** Private function declarations ****/
-
-/**** Public function definitions ****/
-hw::SafeAin::SafeAin(void)
-{
-	return;
-}
-
-hw::SafeAin::~SafeAin(void)
-{
-	return;
-}
-
-void hw::SafeAin::init(bsp::AnalogIn* ain_ch, util::VCounter* timer)
-{
-	this->ain_ch = ain_ch;
-	this->timer = timer;
-	
-	this->under_treshold = 0;
-	this->over_treshold = 0xFFFF;
-	this->hold_time = 0;
-	this->cooldown_time = 0;
-	
-	this->update_ain = 0;
-	this->auto_reset = 0;
-	
-	this->warning = 0;
-	this->fault = 0;
-	
-	this->last_read = 0;
-	
-	this->ts_state_chnage = 0;
-}
-
-void hw::SafeAin::process(void)
-{
-	// Update analog input
-	if(this->update_ain) this->ain_ch->read();
-	
-	this->last_read = this->ain_ch->last_read;
-	
-	// Get current time
-	uint16_t ts_now = this->timer->read();
-	
-	// Update over current and warning condition
-	uint8_t is_outside = 0;
-	if(this->last_read < this->under_treshold) is_outside = 1;
-	if(this->last_read > this->over_treshold) is_outside = 1;
-	
-	// Note start time if new OC condition
-	if(is_outside != this->warning) this->ts_state_chnage = ts_now;
-	
-	// Update warning
-	this->warning = is_outside;
-	
-	// Calculate warning condition time
-	uint16_t td = util::time_delta(this->ts_state_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 ****/
--- a/firmware/src/hw/safe_ain.h
+++ b/firmware/src/hw/safe_ain.h
@@ -1,50 +0,0 @@
-#ifndef SAFE_AIN_H_
-#define SAFE_AIN_H_
-
-/**** Includes ****/
-#include <stdint.h>
-#include "../utils/vcounter.h"
-#include "../bsp/board.h"
-
-namespace hw {
-
-/**** Public definitions ****/
-
-class SafeAin
-{	
-	public:
-	SafeAin(void);
-	~SafeAin(void);
-	
-	void init(bsp::AnalogIn* ain_ch, util::VCounter* timer);
-	
-	uint8_t warning;
-	uint8_t fault;
-	uint16_t last_read;
-	
-	uint16_t under_treshold;
-	uint16_t over_treshold;
-	uint16_t hold_time;
-	uint16_t cooldown_time;
-	
-	uint8_t update_ain;
-	uint8_t auto_reset;
-	
-	void process(void);
-	
-	#ifndef TESTING
-	protected:
-	#endif
-	bsp::AnalogIn* ain_ch;
-	util::VCounter* timer;
-	uint16_t ts_state_chnage;
-};
-
-/**** Public function declarations ****/
-
-#ifdef TESTING
-#endif
-
-} //namespace
-
-#endif /* SAFE_AIN_H_ */
--- a/firmware/src/main.cpp
+++ b/firmware/src/main.cpp
@@ -1,60 +1,43 @@
 /**** Includes ****/
 #include "utils/utils.h"

-#include "dccd/dccd_hw.h"
-#include "dccd/dccd.h"
+#include "board/mcu/mcu_hal.h"
+#include "board/ain.h"
+#include "board/din.h"
+#include "board/dout.h"
+#include "board/dio.h"
+#include "board/halfbridge.h"
+#include "board/pwm.h"
+
+#include "hw/button.h"
+#include "hw/potentiometer.h"
+#include "hw/display_led.h"
+
+#include "board/udccd_board.h"

 /**** Private definitions ****/
 /**** Private constants ****/
 /**** Private variables ****/
-static dccd::DccdHw dccd_hw;
-static dccd::DccdApp app;
-
 /**** Private function declarations ****/
 /**** Public function definitions ****/
 int main(void)
 {
-	// Setup
-	dccd::DccdHw::dccdHwCfg_t cfg;
-	cfg.handbrake_pull_up = 1;
-	cfg.pwm_f_khz = 16;
-	cfg.speed_hall = 0;
-	cfg.counter_step_us = 2000;
-	dccd_hw.init(&cfg);
-	
-	app.init(&dccd_hw);
-	
-	//#define OVERRIDECFG
-	#ifdef OVERRIDECFG
-	// Configuration
-	app.lock_current = 4500;
-	app.max_hbrake_time = 2000;
-	app.button_inputs = 1;
-	app.display_brigth = 100;
-	app.display_dimm = 25;
-	
-	// Initial values
-	app.btn_force = 0;
-	app.brake_mode = 0;
-	#endif
-	
-	// Save config to memory
-	//#define SAVECFG
-	#ifdef SAVECFG
-	app.saveMemCfg();
-	#endif
+	mcu::startupCfg_t mcu_cfg;
+	mcu_cfg.adc_clk = mcu::ADC_DIV2;
+	mcu_cfg.pwm_clk = mcu::TIM_DIV1;
+	mcu_cfg.pwm_top = 200;
+	mcu_cfg.pwm_ch1_en = 1;
 	
+	mcu::startup(&mcu_cfg);
+
 	// Super loop
 	while(1)
-	{	
-		// Do stuff
-		app.process();
-		// End of super loop
-		continue; 
+	{				
+		continue; // End of super loop
 	}
 	
 	// Escape the matrix
 	return 0;
 }

-/**** Private function definitions ***/
+/**** Private function definitions ***/
--- a/firmware/src/uDCCD.cppproj
+++ b/firmware/src/uDCCD.cppproj
@@ -20,10 +20,10 @@
    <OverrideVtor>false</OverrideVtor>
    <CacheFlash>true</CacheFlash>
    <ProgFlashFromRam>true</ProgFlashFromRam>
-    <RamSnippetAddress>0x20000000</RamSnippetAddress>
+    <RamSnippetAddress />
    <UncachedRange />
    <preserveEEPROM>true</preserveEEPROM>
-    <OverrideVtorValue>exception_table</OverrideVtorValue>
+    <OverrideVtorValue />
    <BootSegment>2</BootSegment>
    <ResetRule>0</ResetRule>
    <eraseonlaunchrule>0</eraseonlaunchrule>
@@ -40,157 +40,162 @@
        </dependencies>
      </framework-data>
    </AsfFrameworkConfig>
-    <avrtool>com.atmel.avrdbg.tool.atmelice</avrtool>
-    <avrtoolserialnumber>J42700001490</avrtoolserialnumber>
-    <avrdeviceexpectedsignature>0x1E9516</avrdeviceexpectedsignature>
-    <com_atmel_avrdbg_tool_atmelice>
-      <ToolOptions>
-        <InterfaceProperties>
-          <IspClock>249992</IspClock>
-        </InterfaceProperties>
-        <InterfaceName>ISP</InterfaceName>
-      </ToolOptions>
-      <ToolType>com.atmel.avrdbg.tool.atmelice</ToolType>
-      <ToolNumber>J42700001490</ToolNumber>
-      <ToolName>Atmel-ICE</ToolName>
-    </com_atmel_avrdbg_tool_atmelice>
-    <avrtoolinterface>ISP</avrtoolinterface>
-    <avrtoolinterfaceclock>249992</avrtoolinterfaceclock>
  </PropertyGroup>
  <PropertyGroup Condition=" '$(Configuration)' == 'Release' ">
    <ToolchainSettings>
      <AvrGccCpp>
-  <avrgcc.common.Device>-mmcu=atmega328pb -B "%24(PackRepoDir)\atmel\ATmega_DFP\1.7.374\gcc\dev\atmega328pb"</avrgcc.common.Device>
-  <avrgcc.common.outputfiles.hex>True</avrgcc.common.outputfiles.hex>
-  <avrgcc.common.outputfiles.lss>True</avrgcc.common.outputfiles.lss>
-  <avrgcc.common.outputfiles.eep>True</avrgcc.common.outputfiles.eep>
-  <avrgcc.common.outputfiles.srec>True</avrgcc.common.outputfiles.srec>
-  <avrgcc.common.outputfiles.usersignatures>False</avrgcc.common.outputfiles.usersignatures>
-  <avrgcc.compiler.general.ChangeDefaultCharTypeUnsigned>True</avrgcc.compiler.general.ChangeDefaultCharTypeUnsigned>
-  <avrgcc.compiler.general.ChangeDefaultBitFieldUnsigned>True</avrgcc.compiler.general.ChangeDefaultBitFieldUnsigned>
-  <avrgcc.compiler.symbols.DefSymbols><ListValues>
-  <Value>F_CPU=8000000UL</Value>
-  <Value>NDEBUG</Value>
-</ListValues></avrgcc.compiler.symbols.DefSymbols>
-  <avrgcc.compiler.directories.IncludePaths><ListValues><Value>%24(PackRepoDir)\atmel\ATmega_DFP\1.7.374\include\</Value></ListValues></avrgcc.compiler.directories.IncludePaths>
-  <avrgcc.compiler.optimization.PackStructureMembers>True</avrgcc.compiler.optimization.PackStructureMembers>
-  <avrgcc.compiler.optimization.AllocateBytesNeededForEnum>True</avrgcc.compiler.optimization.AllocateBytesNeededForEnum>
-  <avrgcc.compiler.warnings.AllWarnings>True</avrgcc.compiler.warnings.AllWarnings>
-  <avrgcccpp.compiler.general.ChangeDefaultCharTypeUnsigned>True</avrgcccpp.compiler.general.ChangeDefaultCharTypeUnsigned>
-  <avrgcccpp.compiler.general.ChangeDefaultBitFieldUnsigned>True</avrgcccpp.compiler.general.ChangeDefaultBitFieldUnsigned>
-  <avrgcccpp.compiler.symbols.DefSymbols><ListValues>
-  <Value>F_CPU=8000000UL</Value>
-  <Value>NDEBUG</Value>
-</ListValues></avrgcccpp.compiler.symbols.DefSymbols>
-  <avrgcccpp.compiler.directories.IncludePaths><ListValues><Value>%24(PackRepoDir)\atmel\ATmega_DFP\1.7.374\include\</Value></ListValues></avrgcccpp.compiler.directories.IncludePaths>
-  <avrgcccpp.compiler.optimization.PackStructureMembers>True</avrgcccpp.compiler.optimization.PackStructureMembers>
-  <avrgcccpp.compiler.optimization.AllocateBytesNeededForEnum>True</avrgcccpp.compiler.optimization.AllocateBytesNeededForEnum>
-  <avrgcccpp.compiler.warnings.AllWarnings>True</avrgcccpp.compiler.warnings.AllWarnings>
-  <avrgcccpp.linker.libraries.Libraries><ListValues><Value>libm</Value></ListValues></avrgcccpp.linker.libraries.Libraries>
-  <avrgcccpp.assembler.general.IncludePaths><ListValues><Value>%24(PackRepoDir)\atmel\ATmega_DFP\1.7.374\include\</Value></ListValues></avrgcccpp.assembler.general.IncludePaths>
-  <avrgcc.compiler.optimization.level>Optimize for size (-Os)</avrgcc.compiler.optimization.level>
-  <avrgcccpp.compiler.optimization.level>Optimize for size (-Os)</avrgcccpp.compiler.optimization.level>
-</AvrGccCpp>
+        <avrgcc.common.Device>-mmcu=atmega328pb -B "%24(PackRepoDir)\atmel\ATmega_DFP\1.7.374\gcc\dev\atmega328pb"</avrgcc.common.Device>
+        <avrgcc.common.outputfiles.hex>True</avrgcc.common.outputfiles.hex>
+        <avrgcc.common.outputfiles.lss>True</avrgcc.common.outputfiles.lss>
+        <avrgcc.common.outputfiles.eep>True</avrgcc.common.outputfiles.eep>
+        <avrgcc.common.outputfiles.srec>True</avrgcc.common.outputfiles.srec>
+        <avrgcc.common.outputfiles.usersignatures>False</avrgcc.common.outputfiles.usersignatures>
+        <avrgcc.compiler.general.ChangeDefaultCharTypeUnsigned>True</avrgcc.compiler.general.ChangeDefaultCharTypeUnsigned>
+        <avrgcc.compiler.general.ChangeDefaultBitFieldUnsigned>True</avrgcc.compiler.general.ChangeDefaultBitFieldUnsigned>
+        <avrgcc.compiler.symbols.DefSymbols>
+          <ListValues>
+            <Value>NDEBUG</Value>
+          </ListValues>
+        </avrgcc.compiler.symbols.DefSymbols>
+        <avrgcc.compiler.directories.IncludePaths>
+          <ListValues>
+            <Value>%24(PackRepoDir)\atmel\ATmega_DFP\1.7.374\include\</Value>
+          </ListValues>
+        </avrgcc.compiler.directories.IncludePaths>
+        <avrgcc.compiler.optimization.level>Optimize for size (-Os)</avrgcc.compiler.optimization.level>
+        <avrgcc.compiler.optimization.PackStructureMembers>True</avrgcc.compiler.optimization.PackStructureMembers>
+        <avrgcc.compiler.optimization.AllocateBytesNeededForEnum>True</avrgcc.compiler.optimization.AllocateBytesNeededForEnum>
+        <avrgcc.compiler.warnings.AllWarnings>True</avrgcc.compiler.warnings.AllWarnings>
+        <avrgcccpp.compiler.general.ChangeDefaultCharTypeUnsigned>True</avrgcccpp.compiler.general.ChangeDefaultCharTypeUnsigned>
+        <avrgcccpp.compiler.general.ChangeDefaultBitFieldUnsigned>True</avrgcccpp.compiler.general.ChangeDefaultBitFieldUnsigned>
+        <avrgcccpp.compiler.symbols.DefSymbols>
+          <ListValues>
+            <Value>NDEBUG</Value>
+          </ListValues>
+        </avrgcccpp.compiler.symbols.DefSymbols>
+        <avrgcccpp.compiler.directories.IncludePaths>
+          <ListValues>
+            <Value>%24(PackRepoDir)\atmel\ATmega_DFP\1.7.374\include\</Value>
+          </ListValues>
+        </avrgcccpp.compiler.directories.IncludePaths>
+        <avrgcccpp.compiler.optimization.level>Optimize for size (-Os)</avrgcccpp.compiler.optimization.level>
+        <avrgcccpp.compiler.optimization.PackStructureMembers>True</avrgcccpp.compiler.optimization.PackStructureMembers>
+        <avrgcccpp.compiler.optimization.AllocateBytesNeededForEnum>True</avrgcccpp.compiler.optimization.AllocateBytesNeededForEnum>
+        <avrgcccpp.compiler.warnings.AllWarnings>True</avrgcccpp.compiler.warnings.AllWarnings>
+        <avrgcccpp.linker.libraries.Libraries>
+          <ListValues>
+            <Value>libm</Value>
+          </ListValues>
+        </avrgcccpp.linker.libraries.Libraries>
+        <avrgcccpp.assembler.general.IncludePaths>
+          <ListValues>
+            <Value>%24(PackRepoDir)\atmel\ATmega_DFP\1.7.374\include\</Value>
+          </ListValues>
+        </avrgcccpp.assembler.general.IncludePaths>
+      </AvrGccCpp>
    </ToolchainSettings>
  </PropertyGroup>
  <PropertyGroup Condition=" '$(Configuration)' == 'Debug' ">
    <ToolchainSettings>
      <AvrGccCpp>
-  <avrgcc.common.Device>-mmcu=atmega328pb -B "%24(PackRepoDir)\atmel\ATmega_DFP\1.7.374\gcc\dev\atmega328pb"</avrgcc.common.Device>
-  <avrgcc.common.outputfiles.hex>True</avrgcc.common.outputfiles.hex>
-  <avrgcc.common.outputfiles.lss>True</avrgcc.common.outputfiles.lss>
-  <avrgcc.common.outputfiles.eep>True</avrgcc.common.outputfiles.eep>
-  <avrgcc.common.outputfiles.srec>True</avrgcc.common.outputfiles.srec>
-  <avrgcc.common.outputfiles.usersignatures>False</avrgcc.common.outputfiles.usersignatures>
-  <avrgcc.compiler.general.ChangeDefaultCharTypeUnsigned>True</avrgcc.compiler.general.ChangeDefaultCharTypeUnsigned>
-  <avrgcc.compiler.general.ChangeDefaultBitFieldUnsigned>True</avrgcc.compiler.general.ChangeDefaultBitFieldUnsigned>
-  <avrgcc.compiler.symbols.DefSymbols><ListValues>
-  <Value>F_CPU=8000000UL</Value>
-  <Value>DEBUG</Value>
-</ListValues></avrgcc.compiler.symbols.DefSymbols>
-  <avrgcc.compiler.directories.IncludePaths><ListValues><Value>%24(PackRepoDir)\atmel\ATmega_DFP\1.7.374\include\</Value></ListValues></avrgcc.compiler.directories.IncludePaths>
-  <avrgcc.compiler.optimization.PackStructureMembers>True</avrgcc.compiler.optimization.PackStructureMembers>
-  <avrgcc.compiler.optimization.AllocateBytesNeededForEnum>True</avrgcc.compiler.optimization.AllocateBytesNeededForEnum>
-  <avrgcc.compiler.warnings.AllWarnings>True</avrgcc.compiler.warnings.AllWarnings>
-  <avrgcccpp.compiler.general.ChangeDefaultCharTypeUnsigned>True</avrgcccpp.compiler.general.ChangeDefaultCharTypeUnsigned>
-  <avrgcccpp.compiler.general.ChangeDefaultBitFieldUnsigned>True</avrgcccpp.compiler.general.ChangeDefaultBitFieldUnsigned>
-  <avrgcccpp.compiler.symbols.DefSymbols><ListValues>
-  <Value>F_CPU=8000000UL</Value>
-  <Value>DEBUG</Value>
-</ListValues></avrgcccpp.compiler.symbols.DefSymbols>
-  <avrgcccpp.compiler.directories.IncludePaths><ListValues><Value>%24(PackRepoDir)\atmel\ATmega_DFP\1.7.374\include\</Value></ListValues></avrgcccpp.compiler.directories.IncludePaths>
-  <avrgcccpp.compiler.optimization.PackStructureMembers>True</avrgcccpp.compiler.optimization.PackStructureMembers>
-  <avrgcccpp.compiler.optimization.AllocateBytesNeededForEnum>True</avrgcccpp.compiler.optimization.AllocateBytesNeededForEnum>
-  <avrgcccpp.compiler.warnings.AllWarnings>True</avrgcccpp.compiler.warnings.AllWarnings>
-  <avrgcccpp.linker.libraries.Libraries><ListValues><Value>libm</Value></ListValues></avrgcccpp.linker.libraries.Libraries>
-  <avrgcccpp.assembler.general.IncludePaths><ListValues><Value>%24(PackRepoDir)\atmel\ATmega_DFP\1.7.374\include\</Value></ListValues></avrgcccpp.assembler.general.IncludePaths>
-  <avrgcc.compiler.optimization.level>Optimize debugging experience (-Og)</avrgcc.compiler.optimization.level>
-  <avrgcc.compiler.optimization.DebugLevel>Default (-g2)</avrgcc.compiler.optimization.DebugLevel>
-  <avrgcccpp.compiler.optimization.level>Optimize debugging experience (-Og)</avrgcccpp.compiler.optimization.level>
-  <avrgcccpp.compiler.optimization.DebugLevel>Default (-g2)</avrgcccpp.compiler.optimization.DebugLevel>
-  <avrgcccpp.assembler.debugging.DebugLevel>Default (-Wa,-g)</avrgcccpp.assembler.debugging.DebugLevel>
-</AvrGccCpp>
+        <avrgcc.common.Device>-mmcu=atmega328pb -B "%24(PackRepoDir)\atmel\ATmega_DFP\1.7.374\gcc\dev\atmega328pb"</avrgcc.common.Device>
+        <avrgcc.common.outputfiles.hex>True</avrgcc.common.outputfiles.hex>
+        <avrgcc.common.outputfiles.lss>True</avrgcc.common.outputfiles.lss>
+        <avrgcc.common.outputfiles.eep>True</avrgcc.common.outputfiles.eep>
+        <avrgcc.common.outputfiles.srec>True</avrgcc.common.outputfiles.srec>
+        <avrgcc.common.outputfiles.usersignatures>False</avrgcc.common.outputfiles.usersignatures>
+        <avrgcc.compiler.general.ChangeDefaultCharTypeUnsigned>True</avrgcc.compiler.general.ChangeDefaultCharTypeUnsigned>
+        <avrgcc.compiler.general.ChangeDefaultBitFieldUnsigned>True</avrgcc.compiler.general.ChangeDefaultBitFieldUnsigned>
+        <avrgcc.compiler.symbols.DefSymbols>
+          <ListValues>
+            <Value>DEBUG</Value>
+          </ListValues>
+        </avrgcc.compiler.symbols.DefSymbols>
+        <avrgcc.compiler.directories.IncludePaths>
+          <ListValues>
+            <Value>%24(PackRepoDir)\atmel\ATmega_DFP\1.7.374\include\</Value>
+          </ListValues>
+        </avrgcc.compiler.directories.IncludePaths>
+        <avrgcc.compiler.optimization.level>Optimize debugging experience (-Og)</avrgcc.compiler.optimization.level>
+        <avrgcc.compiler.optimization.PackStructureMembers>True</avrgcc.compiler.optimization.PackStructureMembers>
+        <avrgcc.compiler.optimization.AllocateBytesNeededForEnum>True</avrgcc.compiler.optimization.AllocateBytesNeededForEnum>
+        <avrgcc.compiler.optimization.DebugLevel>Default (-g2)</avrgcc.compiler.optimization.DebugLevel>
+        <avrgcc.compiler.warnings.AllWarnings>True</avrgcc.compiler.warnings.AllWarnings>
+        <avrgcccpp.compiler.general.ChangeDefaultCharTypeUnsigned>True</avrgcccpp.compiler.general.ChangeDefaultCharTypeUnsigned>
+        <avrgcccpp.compiler.general.ChangeDefaultBitFieldUnsigned>True</avrgcccpp.compiler.general.ChangeDefaultBitFieldUnsigned>
+        <avrgcccpp.compiler.symbols.DefSymbols>
+          <ListValues>
+            <Value>DEBUG</Value>
+          </ListValues>
+        </avrgcccpp.compiler.symbols.DefSymbols>
+        <avrgcccpp.compiler.directories.IncludePaths>
+          <ListValues>
+            <Value>%24(PackRepoDir)\atmel\ATmega_DFP\1.7.374\include\</Value>
+          </ListValues>
+        </avrgcccpp.compiler.directories.IncludePaths>
+        <avrgcccpp.compiler.optimization.level>Optimize debugging experience (-Og)</avrgcccpp.compiler.optimization.level>
+        <avrgcccpp.compiler.optimization.PackStructureMembers>True</avrgcccpp.compiler.optimization.PackStructureMembers>
+        <avrgcccpp.compiler.optimization.AllocateBytesNeededForEnum>True</avrgcccpp.compiler.optimization.AllocateBytesNeededForEnum>
+        <avrgcccpp.compiler.optimization.DebugLevel>Default (-g2)</avrgcccpp.compiler.optimization.DebugLevel>
+        <avrgcccpp.compiler.warnings.AllWarnings>True</avrgcccpp.compiler.warnings.AllWarnings>
+        <avrgcccpp.linker.libraries.Libraries>
+          <ListValues>
+            <Value>libm</Value>
+          </ListValues>
+        </avrgcccpp.linker.libraries.Libraries>
+        <avrgcccpp.assembler.general.IncludePaths>
+          <ListValues>
+            <Value>%24(PackRepoDir)\atmel\ATmega_DFP\1.7.374\include\</Value>
+          </ListValues>
+        </avrgcccpp.assembler.general.IncludePaths>
+        <avrgcccpp.assembler.debugging.DebugLevel>Default (-Wa,-g)</avrgcccpp.assembler.debugging.DebugLevel>
+      </AvrGccCpp>
    </ToolchainSettings>
  </PropertyGroup>
  <ItemGroup>
-    <Compile Include="bsp\ain.cpp">
+    <Compile Include="board\ain.cpp">
      <SubType>compile</SubType>
    </Compile>
-    <Compile Include="bsp\ain.h">
+    <Compile Include="board\ain.h">
      <SubType>compile</SubType>
    </Compile>
-    <Compile Include="bsp\ain_lpf.cpp">
+    <Compile Include="board\din.cpp">
      <SubType>compile</SubType>
    </Compile>
-    <Compile Include="bsp\ain_lpf.h">
+    <Compile Include="board\din.h">
      <SubType>compile</SubType>
    </Compile>
-    <Compile Include="bsp\board.cpp">
+    <Compile Include="board\dio.cpp">
      <SubType>compile</SubType>
    </Compile>
-    <Compile Include="bsp\board.h">
+    <Compile Include="board\dio.h">
      <SubType>compile</SubType>
    </Compile>
-    <Compile Include="bsp\din.cpp">
+    <Compile Include="board\halfbridge.cpp">
      <SubType>compile</SubType>
    </Compile>
-    <Compile Include="bsp\din.h">
+    <Compile Include="board\halfbridge.h">
      <SubType>compile</SubType>
    </Compile>
-    <Compile Include="bsp\dout.cpp">
+    <Compile Include="board\mcu\mcu_hal.h">
      <SubType>compile</SubType>
    </Compile>
-    <Compile Include="bsp\dout.h">
+    <Compile Include="board\mcu\mcu_hal_r8.cpp">
      <SubType>compile</SubType>
    </Compile>
-    <Compile Include="bsp\memory.cpp">
+    <Compile Include="board\dout.cpp">
      <SubType>compile</SubType>
    </Compile>
-    <Compile Include="bsp\memory.h">
+    <Compile Include="board\dout.h">
      <SubType>compile</SubType>
    </Compile>
-    <Compile Include="bsp\pwm_out.cpp">
+    <Compile Include="board\pwm.cpp">
      <SubType>compile</SubType>
    </Compile>
-    <Compile Include="bsp\pwm_out.h">
+    <Compile Include="board\pwm.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="dccd\dccd.cpp">
-      <SubType>compile</SubType>
-    </Compile>
-    <Compile Include="dccd\dccd.h">
-      <SubType>compile</SubType>
-    </Compile>
-    <Compile Include="dccd\dccd_hw.cpp">
-      <SubType>compile</SubType>
-    </Compile>
-    <Compile Include="dccd\dccd_hw.h">
+    <Compile Include="board\udccd_board.h">
      <SubType>compile</SubType>
    </Compile>
    <Compile Include="hw\button.cpp">
@@ -199,28 +204,16 @@
    <Compile Include="hw\button.h">
      <SubType>compile</SubType>
    </Compile>
-    <Compile Include="hw\led_display.cpp">
+    <Compile Include="hw\cc_driver.cpp">
      <SubType>compile</SubType>
    </Compile>
-    <Compile Include="hw\led_display.h">
+    <Compile Include="hw\cc_driver.h">
      <SubType>compile</SubType>
    </Compile>
-    <Compile Include="hw\out_reg.cpp">
+    <Compile Include="hw\display_led.cpp">
      <SubType>compile</SubType>
    </Compile>
-    <Compile Include="hw\out_reg.h">
-      <SubType>compile</SubType>
-    </Compile>
-    <Compile Include="hw\safe_ain.cpp">
-      <SubType>compile</SubType>
-    </Compile>
-    <Compile Include="hw\safe_ain.h">
-      <SubType>compile</SubType>
-    </Compile>
-    <Compile Include="hw\out_driver.cpp">
-      <SubType>compile</SubType>
-    </Compile>
-    <Compile Include="hw\out_driver.h">
+    <Compile Include="hw\display_led.h">
      <SubType>compile</SubType>
    </Compile>
    <Compile Include="hw\potentiometer.cpp">
@@ -244,18 +237,11 @@
    <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="board" />
+    <Folder Include="board\mcu" />
    <Folder Include="hw" />
-    <Folder Include="dccd" />
    <Folder Include="utils" />
  </ItemGroup>
  <Import Project="$(AVRSTUDIO_EXE_PATH)\\Vs\\Compiler.targets" />
--- a/firmware/src/utils/interpolate.cpp
+++ b/firmware/src/utils/interpolate.cpp
@@ -131,7 +131,7 @@ uint16_t util::interpolate_2d(uint16_t x, uint16_t y, uint16_t* x_axis, uint8_t
 	return interpolate(y, y0, y1, zy0, zy1);
 }

-uint16_t  util::interpolate(uint16_t x, uint16_t x0, uint16_t x1, uint16_t y0, uint16_t y1)
+uint16_t interpolate(uint16_t x, uint16_t x0, uint16_t x1, uint16_t y0, uint16_t y1)
 {
 	int32_t dy = (int32_t)y1 - (int32_t)y0;
 	int32_t dx = (int32_t)x1 - (int32_t)x0;
@@ -144,7 +144,7 @@ uint16_t  util::interpolate(uint16_t x, uint16_t x0, uint16_t x1, uint16_t y0, u
 	return util::sat_cast(y);
 }

-uint8_t  util::find_interval_end_index(uint16_t val, uint16_t* axis_values, uint8_t len_axis)
+uint8_t find_interval_end_index(uint16_t val, uint16_t* axis_values, uint8_t len_axis)
 {
 	for(uint8_t i=0; i<len_axis; i++)
 	{
@@ -155,7 +155,7 @@ uint8_t  util::find_interval_end_index(uint16_t val, uint16_t* axis_values, uint
 	return len_axis;
 }

-uint16_t  util::index2d_to_index1d(uint8_t ix, uint8_t iy, uint8_t len_x)
+uint16_t index2d_to_index1d(uint8_t ix, uint8_t iy, uint8_t len_x)
 {
 	return ((uint16_t)len_x * iy) + ix;
 }
--- a/firmware/src/utils/utils.cpp
+++ b/firmware/src/utils/utils.cpp
@@ -17,18 +17,6 @@ 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;
@@ -69,6 +57,7 @@ uint16_t util::sat_subtract(uint16_t x, uint16_t y)
 	if(z > x) return 0;
 	else return z;
 }
+
 uint32_t util::sat_subtract(uint32_t x, uint32_t y)
 {
 	uint32_t z = x - y;
@@ -77,25 +66,6 @@ uint32_t util::sat_subtract(uint32_t x, uint32_t y)
 	else return z;
 }

-uint8_t util::abs_subtract(uint8_t x, uint8_t y)
-{
-	if(x > y) return x - y;
-	else return y-x;
-}
-
-uint16_t util::abs_subtract(uint16_t x, uint16_t y)
-{
-	if(x > y) return x - y;
-	else return y-x;
-}
-
-uint32_t util::abs_subtract(uint32_t x, uint32_t y)
-{
-	if(x > y) return x - y;
-	else return y-x;
-}
-
-
 uint16_t util::sat_cast(uint32_t x)
 {
 	if(x > 0x0000FFFF) return 0xFFFF;
@@ -109,46 +79,6 @@ 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;
@@ -204,13 +134,4 @@ uint16_t util::percent_to_16b(uint8_t percent)
 	return pwm;
 }

-uint16_t util::percent_of(uint8_t percent, uint16_t value)
-{
-	if(percent == 0) return 0;
-	else if(percent >= 100) return value;
-	
-	uint32_t temp = (uint32_t)value * percent;
-	return temp/100;
-}
-
 /**** Private function definitions ****/
--- a/firmware/src/utils/utils.h
+++ b/firmware/src/utils/utils.h
@@ -9,8 +9,15 @@ namespace util {
 /**** Public definitions ****/
 /**** Public function declarations ****/
 uint8_t invert(uint8_t x);
-uint16_t invert(uint16_t x);
-uint32_t invert(uint32_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);

 uint8_t sat_add(uint8_t x, uint8_t y);
 uint16_t sat_add(uint16_t x, uint16_t y);
@@ -20,29 +27,8 @@ uint8_t sat_subtract(uint8_t x, uint8_t y);
 uint16_t sat_subtract(uint16_t x, uint16_t y);
 uint32_t sat_subtract(uint32_t x, uint32_t y);

-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 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);
+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);

 #ifdef TESTING
 #endif
--- a/firmware/src/utils/vcounter.cpp
+++ b/firmware/src/utils/vcounter.cpp
@@ -1,67 +0,0 @@
-/**** 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 ****/
-
--- a/firmware/src/utils/vcounter.h
+++ b/firmware/src/utils/vcounter.h
@@ -1,42 +0,0 @@
-#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_ */
--- a/board/udccd_dev_board.eprj
+++ b/board/udccd_dev_board.eprj
Author	SHA1	Message	Date
Andis Zīle	f320bfefb7	Changes	2024-04-10 15:46:26 +03:00
Andis Zīle	8f7e5036e7	Started coil driver class	2024-04-10 15:46:17 +03:00
Andis Zīle	78de20e05b	Created pot class	2024-04-10 15:46:07 +03:00
Andis Zīle	417ecf4128	Created LED display class	2024-04-10 15:46:00 +03:00
Andis Zīle	dda6c7a2ad	Created button class	2024-04-10 15:45:50 +03:00
Andis Zīle	6199f3c43f	Created default mapping	2024-04-10 15:45:32 +03:00
Andis Zīle	6d5c8d226f	Created OD common pwm class	2024-04-10 15:45:17 +03:00
Andis Zīle	0b9d6fa780	Created digital IO classes	2024-04-10 15:45:03 +03:00
Andis Zīle	989d5a1f13	Will redo this	2024-04-10 15:44:41 +03:00
Andis Zīle	c50b3d90bf	board abstraction	2024-04-08 19:40:48 +03:00
Andis Zīle	dd4ff43515	Prepared for Cpp version	2024-03-12 21:27:25 +02:00