From e199a2c2bef8f57c61c408e1f7f7c91be3953aaa Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Andis=20Z=C4=ABle?= <andis.jargans@gmail.com>
Date: Tue, 20 Aug 2024 16:44:35 +0300
Subject: [PATCH] Saved work

---
 firmware/src/bsp/ain.cpp            |   8 +
 firmware/src/bsp/ain.h              |   2 +
 firmware/src/bsp/ain_lpf.cpp        |   4 +
 firmware/src/bsp/board.cpp          |  10 +
 firmware/src/bsp/board.h            |   2 +
 firmware/src/bsp/din.cpp            |  10 +-
 firmware/src/bsp/din.h              |   2 +
 firmware/src/bsp/dout.cpp           |   3 +
 firmware/src/bsp/mcu/mcu_hal.h      |   1 +
 firmware/src/bsp/mcu/mcu_hal_r8.cpp |  30 ++-
 firmware/src/bsp/pwm_out.cpp        |  12 +
 firmware/src/bsp/pwm_out.h          |   2 +
 firmware/src/dccd/brakes.cpp        |   8 +-
 firmware/src/dccd/coil_reg.cpp      |   3 +
 firmware/src/dccd/dccd.cpp          | 334 +++++++++++++++++++++++-----
 firmware/src/dccd/dccd.h            |  45 +++-
 firmware/src/dccd/display.cpp       |  12 +-
 firmware/src/dccd/display.h         |  13 +-
 firmware/src/dccd/handbrake.cpp     |  72 +++++-
 firmware/src/dccd/handbrake.h       |  21 +-
 firmware/src/dccd/memory.cpp        | 257 +++++++++++++++++++++
 firmware/src/dccd/memory.h          |  76 +++++++
 firmware/src/dccd/mode.cpp          |   1 +
 firmware/src/dccd/tps.cpp           |  58 ++++-
 firmware/src/dccd/tps.h             |  15 ++
 firmware/src/dccd/user_force.cpp    |   1 +
 firmware/src/hw/safe_ain.cpp        |  26 ++-
 firmware/src/main.cpp               |  43 +++-
 firmware/src/uDCCD.cppproj          |  12 +-
 firmware/src/utils/vcounter.cpp     |   4 +
 30 files changed, 993 insertions(+), 94 deletions(-)
 create mode 100644 firmware/src/dccd/memory.cpp
 create mode 100644 firmware/src/dccd/memory.h

diff --git a/firmware/src/bsp/ain.cpp b/firmware/src/bsp/ain.cpp
index bfaac3b..fc530a9 100644
--- a/firmware/src/bsp/ain.cpp
+++ b/firmware/src/bsp/ain.cpp
@@ -12,6 +12,7 @@ using namespace bsp;
 /**** Public function definitions ****/
 bsp::AnalogIn::AnalogIn(void)
 {
+	this->is_init_done = 0;
 	return;
 }
 
@@ -27,11 +28,18 @@ void bsp::AnalogIn::init(uint8_t adc_ch)
 	this->div = DEF_AIN_DIV;
 	this->offset = DEF_AIN_OFFSET;
 	this->last_read = 0;
+	this->is_init_done = 1;
 }
 
+uint8_t bsp::AnalogIn::is_init(void)
+{
+	return this->is_init_done;
+}
 
 uint16_t bsp::AnalogIn::read(void)
 {
+	if(this->is_init_done==0) return 0;
+	
 	//Read ADC
 	uint16_t raw = mcu::adc_read(this->adc_ch);
 	
diff --git a/firmware/src/bsp/ain.h b/firmware/src/bsp/ain.h
index 75e8eb0..73050cf 100644
--- a/firmware/src/bsp/ain.h
+++ b/firmware/src/bsp/ain.h
@@ -18,6 +18,7 @@ class AnalogIn
 	~AnalogIn(void);
 	
 	void init(uint8_t adc_ch);
+	uint8_t is_init(void);
 	
 	uint8_t mul;
 	uint8_t div;
@@ -30,6 +31,7 @@ class AnalogIn
 	protected:
 	#endif
 	uint8_t adc_ch;
+	uint8_t is_init_done;
 };
 
 /**** Public function declarations ****/
diff --git a/firmware/src/bsp/ain_lpf.cpp b/firmware/src/bsp/ain_lpf.cpp
index a24f491..03d86df 100644
--- a/firmware/src/bsp/ain_lpf.cpp
+++ b/firmware/src/bsp/ain_lpf.cpp
@@ -12,6 +12,7 @@ using namespace bsp;
 /**** Public function definitions ****/
 bsp::AnalogInLfp::AnalogInLfp(void)
 {
+	this->is_init_done = 0;
 	return;
 }
 
@@ -29,10 +30,13 @@ void bsp::AnalogInLfp::init(uint8_t adc_ch)
 	this->strength = 0;
 	this->last_read = 0;
 	this->last_read_direct = 0;
+	this->is_init_done = 1;
 }
 
 uint16_t bsp::AnalogInLfp::read(void)
 {
+	if(this->is_init_done==0) return 0;
+	
 	//Read ADC
 	uint16_t raw = mcu::adc_read(this->adc_ch);
 	
diff --git a/firmware/src/bsp/board.cpp b/firmware/src/bsp/board.cpp
index 7440429..bab6232 100644
--- a/firmware/src/bsp/board.cpp
+++ b/firmware/src/bsp/board.cpp
@@ -12,6 +12,7 @@ using namespace bsp;
 /**** Public function definitions ****/
 bsp::Board::Board(void)
 {
+	this->is_init_done = 0;
 	return;
 }
 
@@ -82,10 +83,19 @@ void bsp::Board::init(boardCfg_t* cfg)
 	// PWM driver output
 	this->out_pwm.init(mcu::PWM_OUT, 95);
 	this->out_low.init(mcu::GPIO_OUT_LOW, 0);
+	
+	this->is_init_done = 1;
+}
+
+uint8_t bsp::Board::is_init(void)
+{
+	return this->is_init_done;
 }
 
 void bsp::Board::read(void)
 {
+	if(this->is_init_done==0) return;
+	
 	// Update all analog inputs
 	this->out_voltage.read();
 	this->out_current.read();
diff --git a/firmware/src/bsp/board.h b/firmware/src/bsp/board.h
index 97a5d9d..fefd897 100644
--- a/firmware/src/bsp/board.h
+++ b/firmware/src/bsp/board.h
@@ -26,6 +26,7 @@ class Board
 	~Board(void);
 	
 	void init(boardCfg_t* cfg);
+	uint8_t is_init(void);
 	
 	AnalogIn out_voltage;
 	AnalogIn out_current;
@@ -63,6 +64,7 @@ class Board
 	#ifndef TESTING
 	protected:
 	#endif
+	uint8_t is_init_done;
 };
 
 /**** Public function declarations ****/
diff --git a/firmware/src/bsp/din.cpp b/firmware/src/bsp/din.cpp
index dd9b953..1079791 100644
--- a/firmware/src/bsp/din.cpp
+++ b/firmware/src/bsp/din.cpp
@@ -12,6 +12,7 @@ using namespace bsp;
 /**** Public function definitions ****/
 bsp::DigitalIn::DigitalIn(void)
 {
+	this->is_init_done = 0;
 	return;
 }
 
@@ -26,12 +27,19 @@ void bsp::DigitalIn::init(uint8_t gpio_ch, uint8_t inverted)
 	if(inverted == 0) this->is_inverted = 0;
 	else this->is_inverted = 1;
 	this->last_read = 0;
+	this->is_init_done = 1;
 }
 
+uint8_t bsp::DigitalIn::is_init(void)
+{
+	return this->is_init_done;
+}
 
 uint8_t bsp::DigitalIn::read(void)
 {
-	// Read ADC
+	if(this->is_init_done==0) return 0;
+	
+	// Read GPIO
 	this->last_read = mcu::gpio_read(this->gpio_ch);
 	
 	// Invert if necessary
diff --git a/firmware/src/bsp/din.h b/firmware/src/bsp/din.h
index af0080d..333b54e 100644
--- a/firmware/src/bsp/din.h
+++ b/firmware/src/bsp/din.h
@@ -14,6 +14,7 @@ class DigitalIn
 	~DigitalIn(void);
 	
 	void init(uint8_t gpio_ch, uint8_t inverted);
+	uint8_t is_init(void);
 	
 	uint8_t last_read;
 	
@@ -24,6 +25,7 @@ class DigitalIn
 	#endif
 	uint8_t gpio_ch;
 	uint8_t is_inverted;
+	uint8_t is_init_done;
 };
 
 /**** Public function declarations ****/
diff --git a/firmware/src/bsp/dout.cpp b/firmware/src/bsp/dout.cpp
index cdb7f07..e8c9b17 100644
--- a/firmware/src/bsp/dout.cpp
+++ b/firmware/src/bsp/dout.cpp
@@ -12,6 +12,7 @@ using namespace bsp;
 /**** Public function definitions ****/
 bsp::DigitalOut::DigitalOut(void)
 {
+	this->is_init_done = 0;
 	return;
 }
 
@@ -22,6 +23,8 @@ bsp::DigitalOut::~DigitalOut(void)
 
 void bsp::DigitalOut::write(int8_t level)
 {
+	if(this->is_init_done==0) return;
+	
 	if(this->is_inverted)
 	{
 		if(level==0) level = 1;
diff --git a/firmware/src/bsp/mcu/mcu_hal.h b/firmware/src/bsp/mcu/mcu_hal.h
index 187d17d..cddb6d4 100644
--- a/firmware/src/bsp/mcu/mcu_hal.h
+++ b/firmware/src/bsp/mcu/mcu_hal.h
@@ -78,6 +78,7 @@ typedef struct {
 
 /**** Public function declarations ****/
 void startup(startupCfg_t* hwCfg);
+uint8_t is_init(void);
 
 void rtc_set_calibration(uint16_t coef);
 
diff --git a/firmware/src/bsp/mcu/mcu_hal_r8.cpp b/firmware/src/bsp/mcu/mcu_hal_r8.cpp
index 2056d15..3f08d2f 100644
--- a/firmware/src/bsp/mcu/mcu_hal_r8.cpp
+++ b/firmware/src/bsp/mcu/mcu_hal_r8.cpp
@@ -7,8 +7,11 @@ using namespace mcu;
 
 /**** Private definitions ****/
 /**** Private constants ****/
+static const uint8_t def_gpio_read = 0;
+
 /**** Private variables ****/
 static volatile uint16_t rtc_ms = 1000;
+static volatile uint8_t is_init_done = 0;
 
 /**** Private function declarations ****/
 static uint8_t gpio_read_level(uint8_t pin_reg, uint8_t mask);
@@ -18,6 +21,8 @@ static uint16_t pwm_read_ocx(uint8_t ch);
 /**** Public function definitions ****/
 void mcu::startup(startupCfg_t* hwCfg)
 {
+	is_init_done = 0;
+	
 	// Fail-safe GPIO init
 	PORTB = 0xF8;  // Set PORTB pull-ups
 	DDRB = 0x00;   // Set all as inputs
@@ -98,6 +103,13 @@ void mcu::startup(startupCfg_t* hwCfg)
 	
 	uint8_t tim1_prescaler = (uint8_t)hwCfg->pwm_clk;
 	TCCR1B |= tim1_prescaler;  //Enable timer
+	
+	is_init_done = 1;
+}
+
+uint8_t mcu::is_init(void)
+{
+	return is_init_done;
 }
 
 void mcu::rtc_set_calibration(uint16_t coef)
@@ -108,6 +120,8 @@ void mcu::rtc_set_calibration(uint16_t coef)
 // GPIO interface functions
 uint8_t mcu::gpio_read(uint8_t ch)
 {
+	if(is_init_done==0) return def_gpio_read;
+	
 	switch(ch)
 	{
 		case GPIO_DIN1:  // Mode DIN1
@@ -177,12 +191,14 @@ uint8_t mcu::gpio_read(uint8_t ch)
 		return gpio_read_level(PINB,0x10);	
 		
 		default:
-		return 0;
+		return def_gpio_read;
 	}
 }
 
 void mcu::gpio_write(uint8_t ch, int8_t lvl)
 {
+	if(is_init_done==0) return;
+	
 	switch(ch)
 	{		
 		case GPIO_DIN1:  // Mode DIN1
@@ -322,6 +338,8 @@ void mcu::gpio_write(uint8_t ch, int8_t lvl)
 
 void mcu::gpio_write_pull(uint8_t ch, int8_t lvl)
 {
+	if(is_init_done==0) return;
+	
 	switch(ch)
 	{
 		case GPIO_DIN1:  // Mode DIN1
@@ -367,6 +385,8 @@ void mcu::gpio_write_pull(uint8_t ch, int8_t lvl)
 // ADC interface functions
 void mcu::adc_start(uint8_t ch)
 {
+	if(is_init_done==0) return;
+	
 	// check if already running
 	if(ADCSRA&0x40) return;
 	
@@ -399,12 +419,16 @@ uint8_t mcu::adc_is_new(void)
 
 uint16_t mcu::adc_read(void)
 {
+	if(is_init_done==0) return 0;
+	
 	ADCSRA |= 0x10; // Reset int. flag
 	return ADC;
 }
 
 uint16_t mcu::adc_read(uint8_t ch)
 {
+	if(is_init_done==0) return 0;
+	
 	//check if ADC is enabled
 	if(!(ADCSRA&0x80)) return 0xFFFF;
 
@@ -423,6 +447,8 @@ uint16_t mcu::adc_read(uint8_t ch)
 // PWM interface functions
 void mcu::pwm_write(uint8_t ch, uint16_t dc)
 {
+	if(is_init_done==0) return;
+	
 	dc = 0xFFFF - dc;
 	
 	// Calculate value as % of TOP
@@ -440,6 +466,8 @@ void mcu::pwm_write(uint8_t ch, uint16_t dc)
 
 uint16_t mcu::pwm_read(uint8_t ch)
 {
+	if(is_init_done==0) return 0;
+	
 	uint16_t ocrx = pwm_read_ocx(ch);
 	
 	// Check easy answers
diff --git a/firmware/src/bsp/pwm_out.cpp b/firmware/src/bsp/pwm_out.cpp
index 72c34ab..1f34b9f 100644
--- a/firmware/src/bsp/pwm_out.cpp
+++ b/firmware/src/bsp/pwm_out.cpp
@@ -13,6 +13,7 @@ using namespace bsp;
 /**** Public function definitions ****/
 bsp::PwmOut::PwmOut(void)
 {
+	this->is_init_done = 0;
 	return;
 }
 
@@ -30,10 +31,19 @@ void bsp::PwmOut::init(uint8_t pwm_ch, uint8_t max_dc)
 	if(max_dc>100) max_dc = 100;
 	
 	this->max_dc = util::percent_to_16b(max_dc);
+	
+	this->is_init_done = 1;
+}
+
+uint8_t bsp::PwmOut::is_init(void)
+{
+	return this->is_init_done;
 }
 
 void bsp::PwmOut::write(uint16_t numerator)
 {
+	if(this->is_init_done==0) return;
+	
 	// Update target
 	if(numerator > this->max_dc) numerator = this->max_dc;
 	this->last_duty = numerator;
@@ -44,6 +54,8 @@ void bsp::PwmOut::write(uint16_t numerator)
 
 void bsp::PwmOut::write(uint8_t percent)
 {
+	if(this->is_init_done==0) return;
+	
 	// Convert to numerator/0xFFFF
 	this->write(util::percent_to_16b(percent));
 }
diff --git a/firmware/src/bsp/pwm_out.h b/firmware/src/bsp/pwm_out.h
index cdcfec1..bc4bdb5 100644
--- a/firmware/src/bsp/pwm_out.h
+++ b/firmware/src/bsp/pwm_out.h
@@ -14,6 +14,7 @@ class PwmOut
 	~PwmOut(void);
 	
 	void init(uint8_t pwm_ch, uint8_t max_dc);
+	uint8_t is_init(void);
 	
 	void write(uint16_t numerator);
 	void write(uint8_t percent);
@@ -25,6 +26,7 @@ class PwmOut
 	uint8_t pwm_ch;
 	uint16_t last_duty;
 	uint16_t max_dc;
+	uint8_t is_init_done;
 };
 
 /**** Public function declarations ****/
diff --git a/firmware/src/dccd/brakes.cpp b/firmware/src/dccd/brakes.cpp
index 63c0f38..988c5f8 100644
--- a/firmware/src/dccd/brakes.cpp
+++ b/firmware/src/dccd/brakes.cpp
@@ -22,9 +22,11 @@ dccd::Brakes::~Brakes(void)
 void dccd::Brakes::init(dccd::DccdHw* dccd_hw)
 {	
 	this->hardware = dccd_hw;
+	
 	this->mode = OPEN;
-	this->is_active = 0;
 	this->is_new_mode = 0;
+	
+	this->is_active = 0;
 }
 
 void dccd::Brakes::cfg_debounce(uint16_t dbnc_time)
@@ -52,15 +54,19 @@ Brakes::bmode_t dccd::Brakes::cycle_mode(void)
 	{
 		case OPEN:
 		this->mode = KEEP;
+		break;
 		
 		case KEEP:
 		this->mode = LOCK;
+		break;
 		
 		case LOCK:
 		this->mode = OPEN;
+		break;
 		
 		default:
 		this->mode = OPEN;
+		break;
 	}
 	
 	this->is_new_mode = 1;
diff --git a/firmware/src/dccd/coil_reg.cpp b/firmware/src/dccd/coil_reg.cpp
index 4326f60..d342e74 100644
--- a/firmware/src/dccd/coil_reg.cpp
+++ b/firmware/src/dccd/coil_reg.cpp
@@ -21,6 +21,8 @@ dccd::CoilReg::~CoilReg(void)
 
 void dccd::CoilReg::init(dccd::DccdHw* dccd_hw)
 {	
+	this->hardware = dccd_hw;
+	
 	this->lock_current = 4500;
 	this->ref_resistance = 1500;
 	this->cc_max_resistance = 2000;
@@ -90,6 +92,7 @@ void dccd::CoilReg::process(void)
 
 uint8_t dccd::CoilReg::is_fault(void)
 {
+	if(this->disable_protection!=0) return 0;
 	return this->hardware->out_voltage.fault;
 }
 
diff --git a/firmware/src/dccd/dccd.cpp b/firmware/src/dccd/dccd.cpp
index 544205d..6f31da1 100644
--- a/firmware/src/dccd/dccd.cpp
+++ b/firmware/src/dccd/dccd.cpp
@@ -19,57 +19,73 @@ dccd::DccdApp::~DccdApp(void)
 	return;
 }
 
-void dccd::DccdApp::init(DccdHw* dccd_hw)
+void dccd::DccdApp::init(DccdHw* dccd_hw, cfg_app_t* def_cfg)
 {
 	this->hardware = dccd_hw;
+	this->config = def_cfg;
+	
+	// Memory
+	this->nvmem.init(dccd_hw);
+	this->read_nvmem_cfg();
 	
 	// Set config	
 	this->user_force.init(dccd_hw);
-	this->user_force.btn_repeat_time = 300;
-	this->user_force.pot_mode = 0;
-	this->user_force.btn_step = 10;
-	this->user_force.cfg_debounce(10);
+	this->user_force.btn_repeat_time = this->config->user_btn_repeat_time;
+	this->user_force.pot_mode = this->config->pot_mode;
+	this->user_force.btn_step = this->config->btn_step;
+	this->user_force.cfg_debounce(this->config->user_btn_dbnc);
 	
 	this->mode_btn.init(dccd_hw);
-	this->mode_btn.btn_repeat_time = 700;
-	this->mode_btn.cfg_debounce(10);
+	this->mode_btn.btn_repeat_time = this->config->mode_btn_repeat_time;
+	this->mode_btn.cfg_debounce(this->config->user_btn_dbnc);
 	
 	this->tps.init(dccd_hw);
-	this->tps.treshold_on = 65;
-	this->tps.treshold_off = 55;
+	this->tps.treshold_on = this->config->tps_treshold_on;
+	this->tps.treshold_off = this->config->tps_treshold_off;
+	this->tps.timeout_time = this->config->tps_timeout;
+	this->tps.mode = Thtrottle::MODE0;
 	
 	this->hbrake.init(dccd_hw);
-	this->hbrake.latch_time = 1000;
-	this->hbrake.cfg_debounce(100);
+	this->hbrake.latch_time_1 = this->config->hbrake_latch_time_1;
+	this->hbrake.latch_time_2 = this->config->hbrake_latch_time_2;
+	this->hbrake.latch_time_3 = this->config->hbrake_latch_time_3;
+	this->hbrake.mode = Handbrake::LATCH0;
+	this->hbrake.cfg_debounce(this->config->hbrake_dbnc);
 	
 	this->brakes.init(dccd_hw);
-	this->brakes.cfg_debounce(100);
+	this->brakes.mode = Brakes::OPEN;
+	this->brakes.cfg_debounce(this->config->brakes_dbnc);
 	
 	this->coil_reg.init(dccd_hw);
-	this->coil_reg.lock_current = 4500;
-	this->coil_reg.ref_resistance = 1500;
-	this->coil_reg.cc_max_resistance = 2000;
-	this->coil_reg.cc_min_resistance = 1000;
+	this->coil_reg.lock_current = this->config->coil_lock_current;
+	this->coil_reg.ref_resistance = this->config->coil_ref_resistance;
+	this->coil_reg.cc_max_resistance = this->config->coil_out_max_resistance;
+	this->coil_reg.cc_min_resistance = this->config->coil_out_min_resistance;
 	this->coil_reg.target_force = 0;
-	this->coil_reg.disable_protection = 1;
-	this->coil_reg.cfg_set_cc_mode();
+	this->coil_reg.disable_protection = this->config->coil_disable_protection;
+	if(this->config->coil_cc_mode) this->coil_reg.cfg_set_cc_mode();
+	else this->coil_reg.cfg_set_cv_mode();
 	
 	this->dsp.init(dccd_hw);
-	this->dsp.brigth_pwm = 100;
-	this->dsp.dimm_pwm = 25;
+	this->dsp.brigth_pwm = this->config->dsp_brigth_pwm;
+	this->dsp.dimm_pwm = this->config->dsp_dimm_pwm;
 	this->dsp.next_image = 0x01;
 	this->dsp.next_lock_lvl = 0;
 	this->dsp.next_lock_time = 0;
 	
-	// Set variables
-	this->hbrake_force = -1;
-	this->brakes_open_force = -1;
-	this->brakes_lock_force = 100;
-	this->tps_lock_force = 100;
-	
 	// Variable config
-	this->tps_enabled = 1;
-	if(this->user_force.pot_mode!=0) this->tps_enabled = 0;
+	if(this->user_force.pot_mode!=0) this->config->tps_enabled = 0;
+	
+	// Restore saved user config
+	this->user_force.btn_force = this->nvmem.dynamic_cfg.btn_force;
+	if(this->user_force.btn_force > 100)
+	{
+		this->user_force.btn_force = 100;
+		this->nvmem.dynamic_cfg.btn_force = 100;
+	};
+	this->tps.mode = (Thtrottle::tpsmode_t)this->nvmem.dynamic_cfg.tps_mode;
+	this->hbrake.mode = (Handbrake::hbmode_t)this->nvmem.dynamic_cfg.hbrake_mode;
+	this->brakes.mode = (Brakes::bmode_t)this->nvmem.dynamic_cfg.brakes_mode;
 	
 	// Initialize state
 	this->hardware->read();
@@ -87,27 +103,40 @@ void dccd::DccdApp::process(void)
 	// Update all inputs
 	this->hardware->read();
 	
+	// Process pedals
+	this->tps.process();
+	this->hbrake.process();
+	this->brakes.process();
+	
 	// Process mode
 	this->mode_btn.process();
 	if(this->mode_btn.is_new)
 	{
-		this->brakes.cycle_mode();
-		// Save to EEPROM
 		this->mode_btn.is_new = 0;
+		if(this->hbrake.is_active)
+		{
+			this->hbrake.cycle_mode();
+			this->nvmem.dynamic_cfg.hbrake_mode = (uint8_t)this->hbrake.mode;
+		}
+		else if (this->brakes.is_active)
+		{
+			this->brakes.cycle_mode();
+			this->nvmem.dynamic_cfg.brakes_mode = (uint8_t)this->brakes.mode;
+		}
+		else
+		{
+			this->tps.cycle_mode();
+			this->nvmem.dynamic_cfg.tps_mode = (uint8_t)this->tps.mode;
+		}
 	};
 	
 	// Process user force
 	this->user_force.process();
 	
-	//if(this->user_force.is_new_btn_force)
-	//{
-	//	// Save to EEPROM
-	//}
-	
-	// Process pedals
-	this->tps.process();
-	this->hbrake.process();
-	this->brakes.process();
+	if(this->user_force.is_new_btn_force)
+	{
+		this->nvmem.dynamic_cfg.btn_force = this->user_force.btn_force;
+	};
 	
 	// Calculate new output force
 	this->coil_reg.target_force =  this->calc_next_force();
@@ -120,45 +149,105 @@ void dccd::DccdApp::process(void)
 
 	// Execute outputs
 	this->hardware->write();
+	
+	// Save new user config
+	this->nvmem.update();
 }
 
+/**** Private function definitions ***/
+
 int8_t dccd::DccdApp::calc_next_force(void)
 {
 	if(this->hbrake.is_active)
 	{
-		return this->hbrake_force;
+		return this->config->hbrake_force;
 	}
 	else if(this->brakes.is_active)
 	{
 		switch(this->brakes.mode)
 		{
 			case Brakes::OPEN:
-			return this->brakes_open_force;
+			return this->config->brakes_open_force;
 			
 			case Brakes::KEEP:
 			return (int8_t)(this->user_force.force);
 			
 			case Brakes::LOCK:
-			return this->brakes_lock_force;
+			return this->config->brakes_lock_force;
 			
 			default:
 			return 0;
 		}
 	}
-	else if((this->tps_enabled)&&(this->tps.is_active()))
-	{
-		return this->tps_lock_force;
-	}
 	else
 	{
-		return (int8_t)(this->user_force.force);
+		// Determine TPS force override
+		int8_t tps_force = 0;
+		if((this->config->tps_enabled)&&(this->tps.is_active()))
+		{
+			switch(this->tps.mode)
+			{
+				case Thtrottle::MODE0:
+				tps_force = 0;
+				break;
+				
+				case Thtrottle::MODE1:
+				tps_force = this->config->tps_force_1;
+				break;
+				
+				case Thtrottle::MODE2:
+				tps_force = this->config->tps_force_2;
+				break;
+				
+				case Thtrottle::MODE3:
+				tps_force = this->config->tps_force_3;
+				break;
+				
+				default:
+				tps_force = 0;
+				break;
+			}
+		};
+		
+		// Return biggest of two sources
+		if(tps_force > (int8_t)this->user_force.force) return tps_force;
+		else return (int8_t)(this->user_force.force);
 	}
 }
 
 void dccd::DccdApp::dsp_logic(void)
 {
 	// Display image
-	if(this->brakes.is_new_mode)
+	if(this->hbrake.is_new_mode)
+	{
+		this->hbrake.is_new_mode = 0;
+		uint8_t hbmode_image;
+		switch(this->hbrake.mode)
+		{
+			case Handbrake::LATCH0:
+			hbmode_image = 0x07;
+			break;
+			
+			case Handbrake::LATCH1:
+			hbmode_image = 0x0E;
+			break;
+			
+			case Handbrake::LATCH2:
+			hbmode_image = 0x1C;
+			break;
+			
+			case Handbrake::LATCH3:
+			hbmode_image = 0x38;
+			break;
+			
+			default:
+			hbmode_image = 0x07;
+			this->hbrake.mode = Handbrake::LATCH0;
+			break;
+		}
+		this->dsp.write(hbmode_image, 3, this->config->dsp_mode_lock_time);
+	}
+	else if(this->brakes.is_new_mode)
 	{
 		this->brakes.is_new_mode = 0;
 		uint8_t bmode_image;
@@ -181,24 +270,163 @@ void dccd::DccdApp::dsp_logic(void)
 			this->brakes.mode = Brakes::OPEN;
 			break;
 		}
-		this->dsp.write(bmode_image, 3, 2000);
+		this->dsp.write(bmode_image, 3, this->config->dsp_mode_lock_time);
+	}
+	else if(this->tps.is_new_mode)
+	{
+		this->tps.is_new_mode = 0;
+		uint8_t tpsmode_image;
+		switch(this->tps.mode)
+		{
+			case Thtrottle::MODE0:
+			tpsmode_image = 20;
+			break;
+			
+			case Thtrottle::MODE1:
+			tpsmode_image = 60;
+			break;
+			
+			case Thtrottle::MODE2:
+			tpsmode_image = 80;
+			break;
+			
+			case Thtrottle::MODE3:
+			tpsmode_image = 100;
+			break;
+			
+			default:
+			tpsmode_image = 0;
+			this->tps.mode = Thtrottle::MODE0;
+			break;
+		}
+		this->dsp.write_percent(tpsmode_image, DccdDisplay::BAR20, 3, this->config->dsp_mode_lock_time);
 	}
 	else if(this->user_force.is_new_btn_force)
 	{
 		this->user_force.is_new_btn_force = 0;
-		this->dsp.write_percent(this->user_force.force, 0, 2, 1000);
+		this->dsp.write_percent(this->user_force.force, DccdDisplay::DOT10, 2, this->config->dsp_force_lock_time);
 	}
 	else if(this->coil_reg.is_fault())
 	{
-		this->dsp.write(0x21, 1, 0);
+		this->dsp.write(0x33, 1, 0);
 	}
 	else
 	{
-		this->dsp.write_percent(this->coil_reg.read_act_force(), 0, 0, 0);
+		this->dsp.write_percent(this->coil_reg.read_act_force(), DccdDisplay::DOT10, 0, 0);
 	};
 		
 	// Process display
 	this->dsp.process();
 }
 
-/**** Private function definitions ***/
+void dccd::DccdApp::read_nvmem_cfg(void)
+{
+	if(this->config->force_def_config!=0) return;
+	
+	Memory::staticmem_t mem_cfg;
+	this->nvmem.read_static(&mem_cfg);
+	
+	// Process raw saved config
+	if(mem_cfg.is_nvmem_cfg != 0x01) return; // No valid config in memory
+	
+	// Input mode
+	if((mem_cfg.inp_mode == 0x00)||(mem_cfg.inp_mode == 0x01))
+	{
+		this->config->pot_mode = mem_cfg.inp_mode;
+	};
+	
+	// Handbrake
+	this->config->hbrake_latch_time_1 = (uint16_t)mem_cfg.hbrake_t1 * 100;
+	this->config->hbrake_latch_time_2 = (uint16_t)mem_cfg.hbrake_t2 * 100;
+	this->config->hbrake_latch_time_3 = (uint16_t)mem_cfg.hbrake_t3 * 100;
+
+	this->config->hbrake_dbnc = (uint16_t)mem_cfg.hbrake_dbnc * 10;
+	
+	if((mem_cfg.hbrake_force <= 100)||(mem_cfg.hbrake_force == 0xFF))
+	{
+		this->config->hbrake_force = (int8_t)mem_cfg.hbrake_force;
+	};
+	
+	// Brakes
+	this->config->brakes_dbnc = (uint16_t)mem_cfg.brakes_dnbc * 10;
+	
+	if((mem_cfg.brakes_open_force <= 100)||(mem_cfg.brakes_open_force == 0xFF))
+	{
+		this->config->brakes_open_force = (int8_t)mem_cfg.brakes_open_force;
+	};
+	
+	if((mem_cfg.brakes_lock_force <= 100)||(mem_cfg.brakes_lock_force == 0xFF))
+	{
+		this->config->brakes_lock_force = (int8_t)mem_cfg.brakes_lock_force;
+	};
+	
+	// Throttle position
+	if((mem_cfg.tps_en == 0x00)||(mem_cfg.tps_en == 0x01))
+	{
+		this->config->tps_enabled = mem_cfg.tps_en;
+	};
+	
+	if(mem_cfg.tps_on_th <= 100)
+	{
+		this->config->tps_treshold_on = mem_cfg.tps_on_th;
+	};
+	
+	if(mem_cfg.tps_off_th <= 100)
+	{
+		this->config->tps_treshold_off = mem_cfg.tps_off_th;
+	};
+	
+	this->config->tps_timeout = (uint16_t)mem_cfg.tps_timeout * 100;
+	
+	if(mem_cfg.tps_force_1 <= 100)
+	{
+		this->config->tps_force_1 = (int8_t)mem_cfg.tps_force_1;
+	};
+	
+	if(mem_cfg.tps_force_2 <= 100)
+	{
+		this->config->tps_force_2 = (int8_t)mem_cfg.tps_force_2;
+	};
+	
+	if(mem_cfg.tps_force_3 <= 100)
+	{
+		this->config->tps_force_3 = (int8_t)mem_cfg.tps_force_3;
+	};
+	
+	// Coil
+	if(mem_cfg.coil_lock_current <= 60)
+	{
+		this->config->coil_lock_current = (uint16_t)mem_cfg.coil_lock_current * 100;
+	};
+	
+	if((mem_cfg.coil_ccm_resistance >= 10)&&(mem_cfg.coil_ccm_resistance <= 20))
+	{
+		this->config->coil_ref_resistance = (uint16_t)mem_cfg.coil_ccm_resistance * 100;
+	};
+	
+	if((mem_cfg.coil_cvm_resistance >= 10)&&(mem_cfg.coil_cvm_resistance <= 30))
+	{
+		this->config->coil_out_max_resistance = (uint16_t)mem_cfg.coil_cvm_resistance * 100;
+	};
+	
+	if((mem_cfg.coil_protection_dis == 0x00)||(mem_cfg.coil_protection_dis == 0x01))
+	{
+		this->config->coil_disable_protection = mem_cfg.coil_protection_dis;
+	};
+	
+	if((mem_cfg.coil_cc_mode_en == 0x00)||(mem_cfg.coil_cc_mode_en == 0x01))
+	{
+		this->config->coil_cc_mode = mem_cfg.coil_cc_mode_en;
+	};
+	
+	// Display
+	if(mem_cfg.dsp_brigth_pwm <= 100)
+	{
+		this->config->dsp_brigth_pwm = mem_cfg.dsp_brigth_pwm;
+	};
+	
+	if(mem_cfg.dsp_dimm_pwm <= 100)
+	{
+		this->config->dsp_dimm_pwm = mem_cfg.dsp_dimm_pwm;
+	};
+}
diff --git a/firmware/src/dccd/dccd.h b/firmware/src/dccd/dccd.h
index e6e2525..ccf23ed 100644
--- a/firmware/src/dccd/dccd.h
+++ b/firmware/src/dccd/dccd.h
@@ -11,6 +11,7 @@
 #include "coil_reg.h"
 #include "display.h"
 #include "tps.h"
+#include "memory.h"
 
 namespace dccd {
 
@@ -18,10 +19,44 @@ namespace dccd {
 class DccdApp
 {
 	public:
+	typedef struct{
+		uint16_t user_btn_dbnc;
+		uint16_t user_btn_repeat_time;
+		uint8_t pot_mode;
+		uint8_t btn_step;
+		uint16_t mode_btn_repeat_time;
+		uint8_t tps_treshold_on;
+		uint8_t tps_treshold_off;
+		uint16_t tps_timeout;
+		uint16_t hbrake_latch_time_1;
+		uint16_t hbrake_latch_time_2;
+		uint16_t hbrake_latch_time_3;
+		uint16_t hbrake_dbnc;
+		uint16_t brakes_dbnc;
+		uint16_t coil_lock_current;
+		uint16_t coil_ref_resistance;
+		uint16_t coil_out_max_resistance;
+		uint16_t coil_out_min_resistance;
+		uint8_t coil_disable_protection;
+		uint8_t coil_cc_mode;
+		uint8_t dsp_brigth_pwm;
+		uint8_t dsp_dimm_pwm;
+		int8_t hbrake_force;
+		int8_t brakes_open_force;
+		int8_t brakes_lock_force;
+		uint8_t tps_enabled;
+		int8_t tps_force_1;
+		int8_t tps_force_2;
+		int8_t tps_force_3;
+		uint16_t dsp_mode_lock_time;
+		uint16_t dsp_force_lock_time;
+		uint8_t force_def_config;
+	} cfg_app_t;
+	
 	DccdApp(void);
 	~DccdApp(void);
 	
-	void init(DccdHw* dccd_hw);
+	void init(DccdHw* dccd_hw, cfg_app_t* def_cfg);
 	void process(void);
 
 	UserForce user_force;
@@ -31,12 +66,9 @@ class DccdApp
 	CoilReg coil_reg;
 	DccdDisplay dsp;
 	Thtrottle tps;
+	Memory nvmem;
 	
-	int8_t hbrake_force;
-	int8_t brakes_open_force;
-	int8_t brakes_lock_force;
-	uint8_t tps_enabled;
-	int8_t tps_lock_force;
+	cfg_app_t* config;
 	
 	#ifdef TESTING
 	protected:
@@ -45,6 +77,7 @@ class DccdApp
 	
 	int8_t calc_next_force(void);
 	void dsp_logic(void);
+	void read_nvmem_cfg(void);
 };
 
 /**** Public function declarations ****/
diff --git a/firmware/src/dccd/display.cpp b/firmware/src/dccd/display.cpp
index 40a337c..51aa9eb 100644
--- a/firmware/src/dccd/display.cpp
+++ b/firmware/src/dccd/display.cpp
@@ -26,6 +26,7 @@ dccd::DccdDisplay::~DccdDisplay(void)
 void dccd::DccdDisplay::init(dccd::DccdHw* dccd_hw)
 {	
 	this->hardware = dccd_hw;
+	
 	this->brigth_pwm = 50;
 	this->dimm_pwm = 25;
 	
@@ -43,24 +44,22 @@ void dccd::DccdDisplay::cfg_debounce(uint16_t dbnc_time)
 
 void dccd::DccdDisplay::write(uint8_t image, uint8_t lock_lvl, uint16_t lock_time)
 {
-	if(lock_lvl < this->next_lock_lvl) return;
-	
 	this->next_image = image;
 	this->next_lock_lvl = lock_lvl;
 	this->next_lock_time = lock_time;
 }
 
-void dccd::DccdDisplay::write_percent(uint8_t percent, uint8_t style, uint8_t lock_lvl, uint16_t lock_time)
+void dccd::DccdDisplay::write_percent(uint8_t percent, dspstyle_t style, uint8_t lock_lvl, uint16_t lock_time)
 {
 	uint8_t img = 0x01;
 	
 	switch(style)
 	{
-		case 1:
+		case DOT20:
 		img = img_gen_dot20(percent);
 		break;
 		
-		case 2:
+		case BAR20:
 		img = img_gen_bar(percent);
 		break;
 		
@@ -97,10 +96,11 @@ void dccd::DccdDisplay::process(void)
 	{
 		this->act_image = this->next_image;
 		this->act_lock_lvl = this->next_lock_lvl;
+		this->next_lock_lvl = 0;
 		
 		if(this->next_lock_time > 0)
 		{
-			this->hardware->display.write(this->act_image, this->next_lock_time, this->next_lock_time, 1);
+			this->hardware->display.write(this->act_image, this->next_lock_time+1, this->next_lock_time, 1);
 		}
 		else
 		{
diff --git a/firmware/src/dccd/display.h b/firmware/src/dccd/display.h
index daefb77..90d4c33 100644
--- a/firmware/src/dccd/display.h
+++ b/firmware/src/dccd/display.h
@@ -10,7 +10,14 @@ namespace dccd {
 /**** Public definitions ****/
 class DccdDisplay
 {
-	public:	
+	public:
+	typedef enum
+	{
+		DOT10 = 0,
+		DOT20 = 1,
+		BAR20 = 2
+	} dspstyle_t;
+	
 	DccdDisplay(void);
 	~DccdDisplay(void);
 	
@@ -22,10 +29,10 @@ class DccdDisplay
 	
 	uint8_t next_image;
 	uint8_t next_lock_lvl;
-	uint8_t next_lock_time;
+	uint16_t next_lock_time;
 	
 	void write(uint8_t image, uint8_t lock_lvl, uint16_t lock_time);
-	void write_percent(uint8_t percent, uint8_t style, uint8_t lock_lvl, uint16_t lock_time);
+	void write_percent(uint8_t percent, dspstyle_t style, uint8_t lock_lvl, uint16_t lock_time);
 	void force_backlight(uint8_t percent);
 	void process(void);
 
diff --git a/firmware/src/dccd/handbrake.cpp b/firmware/src/dccd/handbrake.cpp
index aa9015c..ca76eb7 100644
--- a/firmware/src/dccd/handbrake.cpp
+++ b/firmware/src/dccd/handbrake.cpp
@@ -22,10 +22,17 @@ dccd::Handbrake::~Handbrake(void)
 void dccd::Handbrake::init(dccd::DccdHw* dccd_hw)
 {	
 	this->hardware = dccd_hw;
-	this->latch_time = 0;
-	this->hbrake_latch = 0;
-	this->start_ts = 0;
+	
+	this->latch_time_1 = 500;
+	this->latch_time_2 = 1000;
+	this->latch_time_3 = 2000;
+	this->mode = LATCH0;
+	this->is_new_mode = 0;
+	
 	this->is_active = 0;
+	
+	this->latch_act = 0;
+	this->start_ts = 0;
 }
 
 void dccd::Handbrake::cfg_debounce(uint16_t dbnc_time)
@@ -33,6 +40,36 @@ void dccd::Handbrake::cfg_debounce(uint16_t dbnc_time)
 	this->hardware->handbrake.dbnc_lim = dbnc_time;
 }
 
+Handbrake::hbmode_t dccd::Handbrake::cycle_mode(void)
+{
+	switch(this->mode)
+	{
+		case LATCH0:
+		this->mode = LATCH1;
+		break;
+		
+		case LATCH1:
+		this->mode = LATCH2;
+		break;
+		
+		case LATCH2:
+		this->mode = LATCH3;
+		break;
+		
+		case LATCH3:
+		this->mode = LATCH0;
+		break;
+		
+		default:
+		this->mode = LATCH0;
+		break;
+	}
+	
+	this->is_new_mode = 1;
+	
+	return this->mode;
+}
+
 uint8_t dccd::Handbrake::process(void)
 {
 	uint16_t ts_now = this->hardware->counter.read();
@@ -44,17 +81,17 @@ uint8_t dccd::Handbrake::process(void)
 			this->hardware->handbrake.is_new = 0;
 			// Note start time
 			this->start_ts = ts_now;
-			this->hbrake_latch = 1;
+			this->latch_act = 1;
 		};
 		this->is_active = 1;
 	}
-	else if((this->hbrake_latch != 0)&&(this->latch_time != 0))
+	else if((this->latch_act != 0)&&(this->act_latch_time() != 0))
 	{
 		uint16_t td = util::time_delta(this->start_ts, ts_now);
 		uint32_t td_ms = this->hardware->counter.convert_ms(td);
-		if(td_ms >= this->latch_time)
+		if(td_ms >= this->act_latch_time())
 		{
-			this->hbrake_latch = 0;
+			this->latch_act = 0;
 			if(this->hardware->handbrake.state > 0) this->is_active = 1;
 			else this->is_active = 0;
 		};
@@ -68,3 +105,24 @@ uint8_t dccd::Handbrake::process(void)
 }
 
 /**** Private function definitions ***/
+uint16_t dccd::Handbrake::act_latch_time(void)
+{
+	switch(this->mode)
+	{
+		case LATCH0:
+		return 0;
+		
+		case LATCH1:
+		return this->latch_time_1;
+		
+		case LATCH2:
+		return this->latch_time_2;
+		
+		case LATCH3:
+		return this->latch_time_3;
+		
+		default:
+		this->mode = LATCH0;
+		return 0;
+	}
+}
\ No newline at end of file
diff --git a/firmware/src/dccd/handbrake.h b/firmware/src/dccd/handbrake.h
index 4728c1e..261194d 100644
--- a/firmware/src/dccd/handbrake.h
+++ b/firmware/src/dccd/handbrake.h
@@ -10,24 +10,39 @@ namespace dccd {
 /**** Public definitions ****/
 class Handbrake
 {
-	public:		
+	public:	
+	typedef enum
+	{
+		LATCH0 = 0,
+		LATCH1 = 1,
+		LATCH2 = 2,
+		LATCH3 = 3
+	}hbmode_t;
+		
 	Handbrake(void);
 	~Handbrake(void);
 	
 	void init(dccd::DccdHw* dccd_hw);
 	
-	uint16_t latch_time;
+	hbmode_t mode;
+	uint16_t latch_time_1;
+	uint16_t latch_time_2;
+	uint16_t latch_time_3;
+	uint8_t is_new_mode;
+	
 	uint8_t is_active;
 		
 	uint8_t process(void);
 	void cfg_debounce(uint16_t dbnc_time);
+	hbmode_t cycle_mode(void);
 	
 	#ifdef TESTING
 	protected:
 	#endif
 	dccd::DccdHw* hardware;
-	uint8_t hbrake_latch;
+	uint8_t latch_act;
 	uint16_t start_ts;
+	uint16_t act_latch_time(void);
 };
 
 /**** Public function declarations ****/
diff --git a/firmware/src/dccd/memory.cpp b/firmware/src/dccd/memory.cpp
new file mode 100644
index 0000000..a6121ba
--- /dev/null
+++ b/firmware/src/dccd/memory.cpp
@@ -0,0 +1,257 @@
+/**** Includes ****/
+#include "../utils/utils.h"
+#include "memory.h"
+
+using namespace dccd;
+
+/**** Private definitions ****/
+/**** Private constants ****/
+static uint16_t static_cfg_addr_offset = 0x0000;	//0-127
+static uint16_t dynamic_cgf_addr_offset = 0x0080;	//128+
+
+/**** Private variables ****/
+/**** Private function declarations ****/
+/**** Public function definitions ****/
+dccd::Memory::Memory(void)
+{
+	return;
+}
+
+dccd::Memory::~Memory(void)
+{
+	return;
+}
+
+void dccd::Memory::init(dccd::DccdHw* dccd_hw)
+{	
+	this->hardware = dccd_hw;
+	this->read_from_nvmem();
+}
+
+void dccd::Memory::update(void)
+{
+	uint16_t addr = dynamic_cgf_addr_offset;
+	
+	if(this->dynamic_cfg.btn_force != this->dyn_cfg_shadow.btn_force)
+	{
+		this->hardware->board_hw.nvmem.write_8b(addr, this->dynamic_cfg.btn_force);
+		this->dyn_cfg_shadow.btn_force = this->dynamic_cfg.btn_force;
+	};
+	addr++;
+	
+	if(this->dynamic_cfg.tps_mode != this->dyn_cfg_shadow.tps_mode)
+	{
+		this->hardware->board_hw.nvmem.write_8b(addr, this->dynamic_cfg.tps_mode);
+		this->dyn_cfg_shadow.tps_mode = this->dynamic_cfg.tps_mode;
+	};
+	addr++;
+	
+	if(this->dynamic_cfg.hbrake_mode != this->dyn_cfg_shadow.hbrake_mode)
+	{
+		this->hardware->board_hw.nvmem.write_8b(addr, this->dynamic_cfg.hbrake_mode);
+		this->dyn_cfg_shadow.hbrake_mode = this->dynamic_cfg.hbrake_mode;
+	};
+	addr++;
+	
+	if(this->dynamic_cfg.brakes_mode != this->dyn_cfg_shadow.brakes_mode)
+	{
+		this->hardware->board_hw.nvmem.write_8b(addr, this->dynamic_cfg.brakes_mode);
+		this->dyn_cfg_shadow.brakes_mode = this->dynamic_cfg.brakes_mode;
+	};
+	addr++;
+}
+
+void dccd::Memory::read_static(staticmem_t* cfg_out)
+{
+	uint16_t addr = static_cfg_addr_offset;
+	
+	cfg_out->is_nvmem_cfg = this->hardware->board_hw.nvmem.read_8b(addr);
+	addr++;
+	
+	cfg_out->inp_mode = this->hardware->board_hw.nvmem.read_8b(addr);
+	addr++;
+	
+	cfg_out->hbrake_t1 = this->hardware->board_hw.nvmem.read_8b(addr);
+	addr++;
+	
+	cfg_out->hbrake_t2 = this->hardware->board_hw.nvmem.read_8b(addr);
+	addr++;
+	
+	cfg_out->hbrake_t3 = this->hardware->board_hw.nvmem.read_8b(addr);
+	addr++;
+	
+	cfg_out->hbrake_dbnc = this->hardware->board_hw.nvmem.read_8b(addr);
+	addr++;
+	
+	cfg_out->brakes_dnbc = this->hardware->board_hw.nvmem.read_8b(addr);
+	addr++;
+	
+	cfg_out->tps_en = this->hardware->board_hw.nvmem.read_8b(addr);
+	addr++;
+	
+	cfg_out->tps_on_th = this->hardware->board_hw.nvmem.read_8b(addr);
+	addr++;
+	
+	cfg_out->tps_off_th = this->hardware->board_hw.nvmem.read_8b(addr);
+	addr++;
+	
+	cfg_out->tps_timeout = this->hardware->board_hw.nvmem.read_8b(addr);
+	addr++;
+	
+	cfg_out->tps_force_1 = this->hardware->board_hw.nvmem.read_8b(addr);
+	addr++;
+	
+	cfg_out->tps_force_2 = this->hardware->board_hw.nvmem.read_8b(addr);
+	addr++;
+	
+	cfg_out->tps_force_3 = this->hardware->board_hw.nvmem.read_8b(addr);
+	addr++;
+	
+	cfg_out->hbrake_force = this->hardware->board_hw.nvmem.read_8b(addr);
+	addr++;
+	
+	cfg_out->brakes_open_force = this->hardware->board_hw.nvmem.read_8b(addr);
+	addr++;
+	
+	cfg_out->brakes_lock_force = this->hardware->board_hw.nvmem.read_8b(addr);
+	addr++;
+	
+	cfg_out->coil_lock_current = this->hardware->board_hw.nvmem.read_8b(addr);
+	addr++;
+	
+	cfg_out->coil_ccm_resistance = this->hardware->board_hw.nvmem.read_8b(addr);
+	addr++;
+	
+	cfg_out->coil_cvm_resistance = this->hardware->board_hw.nvmem.read_8b(addr);
+	addr++;
+	
+	cfg_out->coil_protection_dis = this->hardware->board_hw.nvmem.read_8b(addr);
+	addr++;
+	
+	cfg_out->coil_cc_mode_en = this->hardware->board_hw.nvmem.read_8b(addr);
+	addr++;
+	
+	cfg_out->dsp_brigth_pwm = this->hardware->board_hw.nvmem.read_8b(addr);
+	addr++;
+	
+	cfg_out->dsp_dimm_pwm = this->hardware->board_hw.nvmem.read_8b(addr);
+	addr++;
+}
+
+void dccd::Memory::write_static(staticmem_t* cfg_in)
+{
+	uint16_t addr = static_cfg_addr_offset;
+	
+	this->hardware->board_hw.nvmem.write_8b(addr, cfg_in->is_nvmem_cfg);
+	addr++;
+	
+	this->hardware->board_hw.nvmem.write_8b(addr, cfg_in->inp_mode);;
+	addr++;
+	
+	this->hardware->board_hw.nvmem.write_8b(addr, cfg_in->hbrake_t1);
+	addr++;
+	
+	this->hardware->board_hw.nvmem.write_8b(addr, cfg_in->hbrake_t2);
+	addr++;
+	
+	this->hardware->board_hw.nvmem.write_8b(addr, cfg_in->hbrake_t3);
+	addr++;
+	
+	this->hardware->board_hw.nvmem.write_8b(addr, cfg_in->is_nvmem_cfg);
+	addr++;
+	
+	this->hardware->board_hw.nvmem.write_8b(addr, cfg_in->brakes_dnbc);
+	addr++;
+	
+	this->hardware->board_hw.nvmem.write_8b(addr, cfg_in->tps_en);
+	addr++;
+	
+	this->hardware->board_hw.nvmem.write_8b(addr, cfg_in->tps_on_th);
+	addr++;
+	
+	this->hardware->board_hw.nvmem.write_8b(addr, cfg_in->tps_off_th);
+	addr++;
+	
+	this->hardware->board_hw.nvmem.write_8b(addr, cfg_in->tps_timeout);
+	addr++;
+	
+	this->hardware->board_hw.nvmem.write_8b(addr, cfg_in->tps_force_1);
+	addr++;
+	
+	this->hardware->board_hw.nvmem.write_8b(addr, cfg_in->tps_force_2);
+	addr++;
+	
+	this->hardware->board_hw.nvmem.write_8b(addr, cfg_in->tps_force_3);
+	addr++;
+	
+	this->hardware->board_hw.nvmem.write_8b(addr, cfg_in->hbrake_force);
+	addr++;
+	
+	this->hardware->board_hw.nvmem.write_8b(addr, cfg_in->brakes_open_force);
+	addr++;
+	
+	this->hardware->board_hw.nvmem.write_8b(addr, cfg_in->brakes_lock_force);
+	addr++;
+	
+	this->hardware->board_hw.nvmem.write_8b(addr, cfg_in->coil_lock_current);;
+	addr++;
+	
+	this->hardware->board_hw.nvmem.write_8b(addr, cfg_in->coil_ccm_resistance);;
+	addr++;
+	
+	this->hardware->board_hw.nvmem.write_8b(addr, cfg_in->coil_cvm_resistance);;
+	addr++;
+	
+	this->hardware->board_hw.nvmem.write_8b(addr, cfg_in->coil_protection_dis);
+	addr++;
+	
+	this->hardware->board_hw.nvmem.write_8b(addr, cfg_in->coil_cc_mode_en);
+	addr++;
+	
+	this->hardware->board_hw.nvmem.write_8b(addr, cfg_in->dsp_brigth_pwm);
+	addr++;
+	
+	this->hardware->board_hw.nvmem.write_8b(addr, cfg_in->dsp_dimm_pwm);
+	addr++;
+}
+
+
+/**** Private function definitions ***/
+void dccd::Memory::read_from_nvmem(void)
+{
+	uint16_t addr = dynamic_cgf_addr_offset;
+	
+	this->dyn_cfg_shadow.btn_force = this->hardware->board_hw.nvmem.read_8b(addr);
+	addr++;
+	
+	this->dyn_cfg_shadow.tps_mode = this->hardware->board_hw.nvmem.read_8b(addr);
+	addr++;
+	
+	this->dyn_cfg_shadow.hbrake_mode = this->hardware->board_hw.nvmem.read_8b(addr);
+	addr++;
+	
+	this->dyn_cfg_shadow.brakes_mode = this->hardware->board_hw.nvmem.read_8b(addr);
+	addr++;
+	
+	this->dynamic_cfg.btn_force = this->dyn_cfg_shadow.btn_force;
+	this->dynamic_cfg.tps_mode = this->dyn_cfg_shadow.tps_mode;
+	this->dynamic_cfg.hbrake_mode = this->dyn_cfg_shadow.hbrake_mode;
+	this->dynamic_cfg.brakes_mode = this->dyn_cfg_shadow.brakes_mode;
+}
+
+void dccd::Memory::write_to_nvmem(void)
+{
+	uint16_t addr = dynamic_cgf_addr_offset;
+	
+	this->hardware->board_hw.nvmem.write_8b(addr, this->dyn_cfg_shadow.btn_force);
+	addr++;
+	
+	this->hardware->board_hw.nvmem.write_8b(addr, this->dyn_cfg_shadow.tps_mode);
+	addr++;
+	
+	this->hardware->board_hw.nvmem.write_8b(addr, this->dyn_cfg_shadow.hbrake_mode);
+	addr++;
+	
+	this->hardware->board_hw.nvmem.write_8b(addr, this->dyn_cfg_shadow.brakes_mode);
+	addr++;
+}
diff --git a/firmware/src/dccd/memory.h b/firmware/src/dccd/memory.h
new file mode 100644
index 0000000..1b65222
--- /dev/null
+++ b/firmware/src/dccd/memory.h
@@ -0,0 +1,76 @@
+#ifndef DCCD_MEMORY_H_
+#define DCCD_MEMORY_H_
+
+/**** Includes ****/
+#include <stdint.h>
+#include "dccd_hw.h"
+
+namespace dccd {
+
+/**** Public definitions ****/
+class Memory
+{
+	public:
+	typedef struct{
+		uint8_t is_nvmem_cfg;
+		uint8_t inp_mode;
+		uint8_t hbrake_t1;
+		uint8_t hbrake_t2;
+		uint8_t hbrake_t3;
+		uint8_t hbrake_dbnc;
+		uint8_t hbrake_force;
+		uint8_t brakes_dnbc;
+		uint8_t brakes_open_force;
+		uint8_t brakes_lock_force;
+		uint8_t tps_en;
+		uint8_t tps_on_th;
+		uint8_t tps_off_th;
+		uint8_t tps_timeout;
+		uint8_t tps_force_1;
+		uint8_t tps_force_2;
+		uint8_t tps_force_3;
+		uint8_t coil_lock_current;
+		uint8_t coil_ccm_resistance;
+		uint8_t coil_cvm_resistance;
+		uint8_t coil_protection_dis;
+		uint8_t coil_cc_mode_en;
+		uint8_t dsp_brigth_pwm;
+		uint8_t dsp_dimm_pwm;
+	} staticmem_t;
+	
+	typedef struct{
+		uint8_t btn_force;
+		uint8_t tps_mode;
+		uint8_t hbrake_mode;
+		uint8_t brakes_mode;
+	} dynamicmem_t;
+	
+	Memory(void);
+	~Memory(void);
+	
+	void init(dccd::DccdHw* dccd_hw);
+	
+	dynamicmem_t dynamic_cfg;
+	
+	void update(void);
+	
+	void read_static(staticmem_t* cfg_out);
+	void write_static(staticmem_t* cfg_in);
+	
+	#ifdef TESTING
+	protected:
+	#endif
+	DccdHw* hardware;
+	dynamicmem_t dyn_cfg_shadow;
+	void read_from_nvmem(void);
+	void write_to_nvmem(void);
+};
+
+/**** Public function declarations ****/
+
+#ifdef TESTING
+#endif
+
+} //namespace
+
+#endif /* DCCD_MEMORY_H_ */
\ No newline at end of file
diff --git a/firmware/src/dccd/mode.cpp b/firmware/src/dccd/mode.cpp
index a34b262..cb821f6 100644
--- a/firmware/src/dccd/mode.cpp
+++ b/firmware/src/dccd/mode.cpp
@@ -22,6 +22,7 @@ dccd::ModeBtn::~ModeBtn(void)
 void dccd::ModeBtn::init(dccd::DccdHw* dccd_hw)
 {	
 	this->hardware = dccd_hw;
+	
 	this->btn_repeat_time = 0;
 	this->is_new = 0;
 }
diff --git a/firmware/src/dccd/tps.cpp b/firmware/src/dccd/tps.cpp
index ab0c910..d392b76 100644
--- a/firmware/src/dccd/tps.cpp
+++ b/firmware/src/dccd/tps.cpp
@@ -22,20 +22,70 @@ dccd::Thtrottle::~Thtrottle(void)
 void dccd::Thtrottle::init(dccd::DccdHw* dccd_hw)
 {	
 	this->hardware = dccd_hw;
-	this->is_new = 0;
-	this->active = 0;
 	
 	this->treshold_on = 55;
 	this->treshold_off = 45;
+	this->timeout_time = 0;
+	this->start_ts = 0;
+	this->is_timed_out = 0;
+	
+	this->mode = MODE0;
+	this->is_new_mode = 0;
+	
+	this->is_new = 0;
+	this->active = 0;
+}
+
+Thtrottle::tpsmode_t dccd::Thtrottle::cycle_mode(void)
+{
+	switch(this->mode)
+	{
+		case MODE0:
+		this->mode = MODE1;
+		break;
+		
+		case MODE1:
+		this->mode = MODE2;
+		break;
+		
+		case MODE2:
+		this->mode = MODE3;
+		break;
+		
+		case MODE3:
+		this->mode = MODE0;
+		break;
+		
+		default:
+		this->mode = MODE0;
+		break;
+	}
+	
+	this->is_new_mode = 1;
+	
+	return this->mode;
 }
 
 void dccd::Thtrottle::process(void)
 {
+	uint16_t ts_now = this->hardware->counter.read();
+	
 	if(this->active)
 	{
+		uint16_t td = util::time_delta(this->start_ts, ts_now);
+		uint32_t td_ms = this->hardware->counter.convert_ms(td);
+		
+		if((td_ms >= this->timeout_time)&&(this->timeout_time != 0))
+		{
+			this->is_timed_out = 1;
+			this->is_new = 1;
+		};
+		
 		if(this->hardware->pot.last_percent <= this->treshold_off)
 		{
 			this->active = 0;
+			this->is_timed_out = 0;
+			this->start_ts = 0;
 			this->is_new = 1;
 		}
 		else this->active = 1;
@@ -44,6 +94,7 @@ void dccd::Thtrottle::process(void)
 	{
 		if(this->hardware->pot.last_percent >= this->treshold_on)
 		{
+			this->start_ts = ts_now;
 			this->active = 1;
 			this->is_new = 1;
 		}
@@ -53,7 +104,8 @@ void dccd::Thtrottle::process(void)
 
 uint8_t dccd::Thtrottle::is_active(void)
 {
-	return this->active;
+	if(this->is_timed_out) return 0;
+	else return this->active;
 }
 
 /**** Private function definitions ***/
diff --git a/firmware/src/dccd/tps.h b/firmware/src/dccd/tps.h
index a5ad9ca..7c0da70 100644
--- a/firmware/src/dccd/tps.h
+++ b/firmware/src/dccd/tps.h
@@ -11,6 +11,14 @@ namespace dccd {
 class Thtrottle
 {
 	public:	
+	typedef enum
+	{
+		MODE0 = 0,
+		MODE1 = 1,
+		MODE2 = 2,
+		MODE3 = 3
+	}tpsmode_t;
+	
 	Thtrottle(void);
 	~Thtrottle(void);
 	
@@ -18,16 +26,23 @@ class Thtrottle
 	
 	uint8_t treshold_on;
 	uint8_t treshold_off;
+	uint16_t timeout_time;
+	
+	tpsmode_t mode;
+	uint8_t is_new_mode;
 	
 	uint8_t is_new;
 	uint8_t is_active(void);
 	
 	void process(void);
+	tpsmode_t cycle_mode(void);
 
 	#ifdef TESTING
 	protected:
 	#endif
 	dccd::DccdHw* hardware;
+	uint16_t start_ts;
+	uint8_t is_timed_out;
 	uint8_t active;
 };
 
diff --git a/firmware/src/dccd/user_force.cpp b/firmware/src/dccd/user_force.cpp
index 8691d6a..0c3a7c8 100644
--- a/firmware/src/dccd/user_force.cpp
+++ b/firmware/src/dccd/user_force.cpp
@@ -22,6 +22,7 @@ dccd::UserForce::~UserForce(void)
 void dccd::UserForce::init(dccd::DccdHw* dccd_hw)
 {	
 	this->hardware = dccd_hw;
+	
 	this->is_new_btn_force = 0;
 	this->btn_repeat_time = 0;
 	this->force = 0;
diff --git a/firmware/src/hw/safe_ain.cpp b/firmware/src/hw/safe_ain.cpp
index 5cbc54e..9bb730e 100644
--- a/firmware/src/hw/safe_ain.cpp
+++ b/firmware/src/hw/safe_ain.cpp
@@ -57,17 +57,37 @@ void hw::SafeAin::process(void)
 	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;
+	if((is_outside!=0)&&(this->warning==0))
+	{
+		this->ts_state_chnage = ts_now;
+	};
 	
 	// Update warning
-	this->warning = is_outside;
+	if(this->warning)
+	{
+		if(is_outside == 0)
+		{
+			this->warning = 0;
+		};
+	}
+	else
+	{
+		if(is_outside == 1)
+		{
+			this->warning = 1;
+		};
+	}
+	
+	//this->warning = is_outside;
+	
+	if((this->warning==0)&&(this->fault==0)) return;
 	
 	// 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))
+	if((this->fault==0)&&(time_ms > (uint32_t)this->hold_time)&&(this->warning!=0))
 	{
 		this->fault = 1;
 		return;
diff --git a/firmware/src/main.cpp b/firmware/src/main.cpp
index 27c4bb1..eff4540 100644
--- a/firmware/src/main.cpp
+++ b/firmware/src/main.cpp
@@ -8,13 +8,14 @@
 /**** Private constants ****/
 /**** Private variables ****/
 static dccd::DccdHw dccd_hw;
-static dccd::DccdApp app;
+static dccd::DccdApp dccd_app;
+static dccd::DccdApp::cfg_app_t def_cfg;
 
 /**** Private function declarations ****/
 /**** Public function definitions ****/
 int main(void)
 {
-	// Setup
+	// Hardware setup
 	dccd::DccdHw::dccdHwCfg_t cfg;
 	cfg.handbrake_pull_up = 1;
 	cfg.pwm_f_khz = 16;
@@ -22,13 +23,47 @@ int main(void)
 	cfg.counter_step_us = 2000;
 	dccd_hw.init(&cfg);
 	
-	app.init(&dccd_hw);
+	// App default config
+	def_cfg.user_btn_dbnc = 10;
+	def_cfg.user_btn_repeat_time = 500;
+	def_cfg.pot_mode = 0;
+	def_cfg.btn_step = 10;
+	def_cfg.mode_btn_repeat_time = 700;
+	def_cfg.tps_treshold_on = 65;
+	def_cfg.tps_treshold_off = 55;
+	def_cfg.tps_timeout = 0;
+	def_cfg.hbrake_latch_time_1 = 500;
+	def_cfg.hbrake_latch_time_2 = 1000;
+	def_cfg.hbrake_latch_time_3 = 2000;
+	def_cfg.hbrake_dbnc = 100;
+	def_cfg.brakes_dbnc = 100;
+	def_cfg.coil_lock_current = 4500;
+	def_cfg.coil_ref_resistance = 1500;
+	def_cfg.coil_out_max_resistance = 2000;
+	def_cfg.coil_out_min_resistance = 1000;
+	def_cfg.coil_disable_protection = 0;
+	def_cfg.coil_cc_mode = 1;
+	def_cfg.dsp_brigth_pwm = 40;
+	def_cfg.dsp_dimm_pwm = 25;
+	def_cfg.hbrake_force = -1;
+	def_cfg.brakes_open_force = -1;
+	def_cfg.brakes_lock_force = 100;
+	def_cfg.tps_enabled = 1;
+	def_cfg.tps_force_1 = 60;
+	def_cfg.tps_force_2 = 80;
+	def_cfg.tps_force_3 = 100;
+	def_cfg.dsp_mode_lock_time = 2000;
+	def_cfg.dsp_force_lock_time = 1000;
+	def_cfg.force_def_config = 0;
+	
+	// App setup
+	dccd_app.init(&dccd_hw, &def_cfg);
 	
 	// Super loop
 	while(1)
 	{	
 		// Do stuff
-		app.process();
+		dccd_app.process();
 		// End of super loop
 		continue; 
 	}
diff --git a/firmware/src/uDCCD.cppproj b/firmware/src/uDCCD.cppproj
index d5d697d..81e0c5f 100644
--- a/firmware/src/uDCCD.cppproj
+++ b/firmware/src/uDCCD.cppproj
@@ -26,7 +26,7 @@
     <OverrideVtorValue>exception_table</OverrideVtorValue>
     <BootSegment>2</BootSegment>
     <ResetRule>0</ResetRule>
-    <eraseonlaunchrule>0</eraseonlaunchrule>
+    <eraseonlaunchrule>1</eraseonlaunchrule>
     <EraseKey />
     <AsfFrameworkConfig>
       <framework-data xmlns="">
@@ -48,13 +48,13 @@
         <InterfaceProperties>
           <IspClock>249992</IspClock>
         </InterfaceProperties>
-        <InterfaceName>ISP</InterfaceName>
+        <InterfaceName>debugWIRE</InterfaceName>
       </ToolOptions>
       <ToolType>com.atmel.avrdbg.tool.atmelice</ToolType>
       <ToolNumber>J42700001490</ToolNumber>
       <ToolName>Atmel-ICE</ToolName>
     </com_atmel_avrdbg_tool_atmelice>
-    <avrtoolinterface>ISP</avrtoolinterface>
+    <avrtoolinterface>debugWIRE</avrtoolinterface>
     <avrtoolinterfaceclock>249992</avrtoolinterfaceclock>
   </PropertyGroup>
   <PropertyGroup Condition=" '$(Configuration)' == 'Release' ">
@@ -253,6 +253,12 @@
     <Compile Include="dccd\handbrake.h">
       <SubType>compile</SubType>
     </Compile>
+    <Compile Include="dccd\memory.cpp">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="dccd\memory.h">
+      <SubType>compile</SubType>
+    </Compile>
     <Compile Include="dccd\mode.cpp">
       <SubType>compile</SubType>
     </Compile>
diff --git a/firmware/src/utils/vcounter.cpp b/firmware/src/utils/vcounter.cpp
index 6870cc1..fcc4044 100644
--- a/firmware/src/utils/vcounter.cpp
+++ b/firmware/src/utils/vcounter.cpp
@@ -11,6 +11,10 @@ using namespace util;
 /**** Public function definitions ****/
 util::VCounter::VCounter(void)
 {
+	this->counter = 0;
+	this->top = 0xFFFF;
+	this->step_us = 1;
+	this->disabled = 1;
 	return;
 }