Saved work

Added mode translators
Saved work
2024-12-03 09:17:59 +02:00 · 2024-09-17 23:24:01 +03:00 · 2024-08-20 16:44:35 +03:00 · 2024-08-02 10:18:09 +03:00
34 changed files with 2210 additions and 406 deletions
--- 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);
 	
--- 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 ****/
--- 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);
 	
--- 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();
--- 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 ****/
--- 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
--- 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 ****/
--- 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;
--- 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);

--- 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
--- 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));
 }
--- 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 ****/
--- a/firmware/src/dccd/brakes.cpp
+++ b/firmware/src/dccd/brakes.cpp
@@ -0,0 +1,117 @@
+/**** Includes ****/
+#include "../utils/utils.h"
+#include "brakes.h"
+
+using namespace dccd;
+
+/**** Private definitions ****/
+/**** Private constants ****/
+/**** Private variables ****/
+/**** Private function declarations ****/
+/**** Public function definitions ****/
+dccd::Brakes::Brakes(void)
+{
+	return;
+}
+
+dccd::Brakes::~Brakes(void)
+{
+	return;
+}
+
+void dccd::Brakes::init(dccd::DccdHw* dccd_hw)
+{	
+	this->hardware = dccd_hw;
+	
+	this->mode = OPEN;
+	this->is_new_mode = 0;
+	
+	this->is_active = 0;
+}
+
+void dccd::Brakes::cfg_debounce(uint16_t dbnc_time)
+{
+	this->hardware->brakes.dbnc_lim = dbnc_time;
+}
+
+uint8_t dccd::Brakes::process(void)
+{
+	if(this->hardware->brakes.state > 0)
+	{
+		this->is_active = 1;
+	}
+	else
+	{
+		this->is_active = 0;
+	}
+	
+	return this->is_active;
+}
+
+Brakes::bmode_t dccd::Brakes::cycle_mode(void)
+{
+	switch(this->mode)
+	{
+		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;
+	
+	return this->mode;
+}
+
+uint8_t dccd::Brakes::get_mode_int(void)
+{
+	switch(this->mode)
+	{
+		case OPEN:
+		return 0;
+		
+		case KEEP:
+		return 1;
+		
+		case LOCK:
+		return 2;
+		
+		default:
+		return 0;
+	}
+}
+
+void dccd::Brakes::set_mode_int(uint8_t mode_int)
+{
+	switch(mode_int)
+	{
+		case 0:
+		this->mode = OPEN;
+		break;
+		
+		case 1:
+		this->mode = KEEP;
+		break;
+		
+		case 2:
+		this->mode = LOCK;
+		break;
+		
+		default:
+		this->mode = OPEN;
+		break;
+	}
+}
+
+/**** Private function definitions ***/
--- a/firmware/src/dccd/brakes.h
+++ b/firmware/src/dccd/brakes.h
@@ -0,0 +1,49 @@
+#ifndef DCCD_BRAKES_H_
+#define DCCD_BRAKES_H_
+
+/**** Includes ****/
+#include <stdint.h>
+#include "dccd_hw.h"
+
+namespace dccd {
+
+/**** Public definitions ****/
+class Brakes
+{
+	public:
+	typedef enum
+	{
+		OPEN = 0,
+		KEEP = 1,
+		LOCK = 2
+	}bmode_t;
+	
+	Brakes(void);
+	~Brakes(void);
+	
+	void init(dccd::DccdHw* dccd_hw);	
+	
+	bmode_t mode;
+	uint8_t is_active;
+	uint8_t is_new_mode;
+	
+	void cfg_debounce(uint16_t dbnc_time);
+	bmode_t cycle_mode(void);
+	uint8_t process(void);
+	uint8_t get_mode_int(void);
+	void set_mode_int(uint8_t mode_int);
+	
+	#ifdef TESTING
+	protected:
+	#endif
+	dccd::DccdHw* hardware;
+};
+
+/**** Public function declarations ****/
+
+#ifdef TESTING
+#endif
+
+} //namespace
+
+#endif /* DCCD_BRAKES_H_ */
--- a/firmware/src/dccd/coil_reg.cpp
+++ b/firmware/src/dccd/coil_reg.cpp
@@ -0,0 +1,115 @@
+/**** Includes ****/
+#include "../utils/utils.h"
+#include "coil_reg.h"
+
+using namespace dccd;
+
+/**** Private definitions ****/
+/**** Private constants ****/
+/**** Private variables ****/
+/**** Private function declarations ****/
+/**** Public function definitions ****/
+dccd::CoilReg::CoilReg(void)
+{
+	return;
+}
+
+dccd::CoilReg::~CoilReg(void)
+{
+	return;
+}
+
+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;
+	this->cc_min_resistance = 1000;
+	this->target_force = 0;
+	this->set_force = 0;
+	this->disable_protection = 0;
+	
+	// Config output protection
+	this->hardware->out_voltage.under_treshold = 0;
+	this->hardware->out_voltage.over_treshold = 0xFFFF;
+	this->hardware->out_voltage.hold_time = 200;
+	this->hardware->out_voltage.cooldown_time = 1000;
+	this->hardware->out_voltage.auto_reset = 1;
+}
+
+void dccd::CoilReg::process(void)
+{	
+	// Fix target force
+	if(this->target_force > 100) this->target_force = 100;
+	
+	// Check protection
+	if((this->disable_protection==0)&&(this->hardware->out_voltage.fault!=0))
+	{
+		// HiZ
+		this->hardware->outreg.write_voltage(0);
+		this->hardware->outreg.write_current(0);
+		this->hardware->outreg.write_on(0);
+		this->hardware->out_voltage.under_treshold = 0;
+		this->set_force = -1;
+		return;
+	};
+
+	// Check for target changes
+	if(this->target_force == this->set_force) return;
+	
+	// Update set force
+	this->set_force = this->target_force;
+	if(this->set_force < 0)
+	{
+		// HiZ
+		this->hardware->outreg.write_voltage(0);
+		this->hardware->outreg.write_current(0);
+		this->hardware->outreg.write_on(0);
+		this->hardware->out_voltage.under_treshold = 0;
+	}
+	else if(this->set_force == 0)
+	{
+		// Open
+		this->hardware->outreg.write_voltage(0);
+		this->hardware->outreg.write_current(0);
+		this->hardware->outreg.write_on(1);
+		this->hardware->out_voltage.under_treshold = 0;
+	}
+	else
+	{
+		// Calculate current and voltage settings
+		this->hardware->outreg.write_current(util::percent_of((uint8_t)this->set_force, this->lock_current));
+		uint16_t resistance = this->ref_resistance;
+		if(this->hardware->outreg.cc_mode_en) resistance = this->cc_max_resistance;
+		this->hardware->outreg.write_voltage(util::sat_mul_kilo(this->hardware->outreg.read_current(), resistance));
+		this->hardware->outreg.write_on(1);
+		// Calculate min. voltage
+		if(this->disable_protection==0) this->hardware->out_voltage.under_treshold = util::sat_mul_kilo(this->hardware->outreg.read_current(), cc_min_resistance);
+	}
+}
+
+uint8_t dccd::CoilReg::is_fault(void)
+{
+	if(this->disable_protection!=0) return 0;
+	return this->hardware->out_voltage.fault;
+}
+
+uint8_t dccd::CoilReg::read_act_force(void)
+{
+	if(this->set_force < 0) return 0;
+	else return (uint8_t)this->set_force;
+}
+
+void dccd::CoilReg::cfg_set_cv_mode(void)
+{
+	this->hardware->outreg.cc_mode_en = 0;
+}
+
+void dccd::CoilReg::cfg_set_cc_mode(void)
+{
+	this->hardware->outreg.cc_mode_en = 1;
+}
+
+/**** Private function definitions ***/
--- a/firmware/src/dccd/coil_reg.h
+++ b/firmware/src/dccd/coil_reg.h
@@ -0,0 +1,47 @@
+#ifndef DCCD_COIL_REG_H_
+#define DCCD_COIL_REG_H_
+
+/**** Includes ****/
+#include <stdint.h>
+#include "dccd_hw.h"
+
+namespace dccd {
+
+/**** Public definitions ****/
+class CoilReg
+{
+	public:	
+	CoilReg(void);
+	~CoilReg(void);
+	
+	void init(dccd::DccdHw* dccd_hw);
+	
+	uint16_t lock_current;
+	uint16_t ref_resistance;
+	uint16_t cc_max_resistance;
+	uint16_t cc_min_resistance;
+	uint8_t disable_protection;
+	
+	int8_t target_force;
+	
+	void process(void);
+	uint8_t read_act_force(void);
+	void cfg_set_cv_mode(void);
+	void cfg_set_cc_mode(void);
+	uint8_t is_fault(void);
+
+	#ifdef TESTING
+	protected:
+	#endif
+	dccd::DccdHw* hardware;
+	int8_t set_force;
+};
+
+/**** Public function declarations ****/
+
+#ifdef TESTING
+#endif
+
+} //namespace
+
+#endif /* DCCD_COIL_REG_H_ */
--- a/firmware/src/dccd/dccd.cpp
+++ b/firmware/src/dccd/dccd.cpp
@@ -6,48 +6,8 @@ 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)
 {
@@ -59,37 +19,82 @@ 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;
 	
-	#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
+	// Memory
+	this->nvmem.init(dccd_hw);
+	this->read_nvmem_cfg();
 	
-	// Load saved config from memory
-	this->loadMemCfg();
+	// Set config	
+	this->user_force.init(dccd_hw);
+	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->btn_force_repeat_time = def_btn_force_repeat_time;
-	this->btn_mode_repeat_time = def_btn_mode_repeat_time;
-	this->pot_force = 0;
+	this->mode_btn.init(dccd_hw);
+	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 = 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_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.mode = Brakes::OPEN;
+	this->brakes.cfg_debounce(this->config->brakes_dbnc);
+	
+	this->coil_reg.init(dccd_hw);
+	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 = 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 = 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;
+	
+	// Variable config
+	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.set_mode_int(this->nvmem.dynamic_cfg.tps_mode);
+	this->hbrake.set_mode_int(this->nvmem.dynamic_cfg.hbrake_mode);
+	this->brakes.set_mode_int(this->nvmem.dynamic_cfg.brakes_mode);
+	
+	// Initialize state
 	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->coil_reg.process();
+	
+	this->dsp.force_backlight(this->dsp.brigth_pwm);
+	this->dsp.process();
 	
 	this->hardware->write();
 }
@@ -99,334 +104,335 @@ 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 pedals
+	this->tps.process();
+	this->hbrake.process();
+	this->brakes.process();
 	
-	// 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)))
+	// Process mode
+	this->mode_btn.process();
+	if(this->mode_btn.is_new)
 	{
-		this->hardware->btn_mode.time_reset();
-		this->hardware->btn_mode.is_new = 0;
-		// Change mode
-		switch(this->brake_mode)
+		this->mode_btn.is_new = 0;
+		if(this->hbrake.is_active)
 		{
-			case 0:
-			this->brake_mode = 1;
-			break;
-			
-			case 1:
-			this->brake_mode = 2;
-			break;
-			
-			default:
-			this->brake_mode = 0;
-			break;
+			this->hbrake.cycle_mode();
+			this->nvmem.dynamic_cfg.hbrake_mode = this->hbrake.get_mode_int();
 		}
-		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)
+		else if (this->brakes.is_active)
 		{
-			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;
+			this->brakes.cycle_mode();
+			this->nvmem.dynamic_cfg.brakes_mode = this->brakes.get_mode_int();
 		}
-	}
-	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)
+		else if(this->user_force.pot_mode==0)
 		{
-			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->tps.cycle_mode();
+			this->nvmem.dynamic_cfg.tps_mode = this->tps.get_mode_int();
+		}
+		else
+		{
+			this->brakes.cycle_mode();
+			this->nvmem.dynamic_cfg.brakes_mode = this->brakes.get_mode_int();
 		}
-		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)
+	// Process user force
+	this->user_force.process();
+	
+	if(this->user_force.is_new_btn_force)
 	{
-		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);
+		this->nvmem.dynamic_cfg.btn_force = this->user_force.btn_force;
 	};
 	
+	// Calculate new output force
+	this->coil_reg.target_force =  this->calc_next_force();
+	
+	// Process coil driver
+	this->coil_reg.process();
+	
+	// Process display logic
+	this->dsp_logic();
+
 	// 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);
+	// Save new user config
+	this->nvmem.update();
 }

 /**** Private function definitions ***/
-static uint8_t img_gen_dot10(uint8_t percent)
+
+int8_t dccd::DccdApp::calc_next_force(void)
 {
-	switch(percent)
+	if(this->hbrake.is_active)
 	{
-		case 0 ... 5:
-		return 0x01;
+		return this->config->hbrake_force;
+	}
+	else if(this->brakes.is_active)
+	{
+		switch(this->brakes.mode)
+		{
+			case Brakes::OPEN:
+			return this->config->brakes_open_force;
+			
+			case Brakes::KEEP:
+			return (int8_t)(this->user_force.force);
+			
+			case Brakes::LOCK:
+			return this->config->brakes_lock_force;
+			
+			default:
+			return 0;
+		}
+	}
+	else
+	{
+		// 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;
+			}
+		};
 		
-		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;
+		// 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);
 	}
 }

-static uint8_t img_gen_dot20(uint8_t percent)
+void dccd::DccdApp::dsp_logic(void)
 {
-	switch(percent)
+	// Display image
+	if(this->hbrake.is_new_mode)
 	{
-		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;
+		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;
+		switch(this->brakes.mode)
+		{
+			case Brakes::OPEN:
+			bmode_image = 0x07;
+			break;
+				
+			case Brakes::KEEP:
+			bmode_image = 0x1E;
+			break;
+				
+			case Brakes::LOCK:
+			bmode_image = 0x38;
+			break;
+				
+			default:
+			bmode_image = 0x07;
+			this->brakes.mode = Brakes::OPEN;
+			break;
+		}
+		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, DccdDisplay::DOT10, 2, this->config->dsp_force_lock_time);
+	}
+	else if(this->coil_reg.is_fault())
+	{
+		this->dsp.write(0x33, 1, 0);
+	}
+	else
+	{
+		this->dsp.write_percent(this->coil_reg.read_act_force(), DccdDisplay::DOT10, 0, 0);
+	};
+		
+	// Process display
+	this->dsp.process();
 }

-static uint8_t img_gen_bar(uint8_t percent)
+void dccd::DccdApp::read_nvmem_cfg(void)
 {
-	switch(percent)
+	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))
 	{
-		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;
-	}
+		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;
+	};
 }
--- a/firmware/src/dccd/dccd.h
+++ b/firmware/src/dccd/dccd.h
@@ -4,6 +4,14 @@
 /**** Includes ****/
 #include <stdint.h>
 #include "dccd_hw.h"
+#include "user_force.h"
+#include "mode.h"
+#include "handbrake.h"
+#include "brakes.h"
+#include "coil_reg.h"
+#include "display.h"
+#include "tps.h"
+#include "memory.h"

 namespace dccd {

@@ -11,31 +19,65 @@ 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;
+	ModeBtn mode_btn;
+	Handbrake hbrake;
+	Brakes brakes;
+	CoilReg coil_reg;
+	DccdDisplay dsp;
+	Thtrottle tps;
+	Memory nvmem;
 	
-	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);
+	cfg_app_t* config;
 	
 	#ifdef TESTING
 	protected:
 	#endif
 	DccdHw* hardware;
+	
+	int8_t calc_next_force(void);
+	void dsp_logic(void);
+	void read_nvmem_cfg(void);
 };

 /**** Public function declarations ****/
--- a/firmware/src/dccd/display.cpp
+++ b/firmware/src/dccd/display.cpp
@@ -0,0 +1,208 @@
+/**** Includes ****/
+#include "../utils/utils.h"
+#include "display.h"
+
+using namespace dccd;
+
+/**** 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 ****/
+dccd::DccdDisplay::DccdDisplay(void)
+{
+	return;
+}
+
+dccd::DccdDisplay::~DccdDisplay(void)
+{
+	return;
+}
+
+void dccd::DccdDisplay::init(dccd::DccdHw* dccd_hw)
+{	
+	this->hardware = dccd_hw;
+	
+	this->brigth_pwm = 50;
+	this->dimm_pwm = 25;
+	
+	this->next_image = 0x00;
+	this->next_lock_lvl = 0;
+	this->next_lock_time = 0;
+	this->act_image = 0x00;
+	this->act_lock_lvl = 0;
+}
+
+void dccd::DccdDisplay::cfg_debounce(uint16_t dbnc_time)
+{
+	this->hardware->dimm.dbnc_lim = dbnc_time;
+}
+
+void dccd::DccdDisplay::write(uint8_t image, uint8_t lock_lvl, uint16_t lock_time)
+{
+	this->next_image = image;
+	this->next_lock_lvl = lock_lvl;
+	this->next_lock_time = 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 DOT20:
+		img = img_gen_dot20(percent);
+		break;
+		
+		case BAR20:
+		img = img_gen_bar(percent);
+		break;
+		
+		default:
+		img = img_gen_dot10(percent);
+		break;
+	}
+	
+	this->write(img, lock_lvl, lock_time);
+}
+
+void dccd::DccdDisplay::force_backlight(uint8_t percent)
+{
+	if(percent > this->brigth_pwm) percent = this->brigth_pwm;
+	this->hardware->display.write_backlight(percent);
+}
+
+void dccd::DccdDisplay::process(void)
+{	
+	// Process DIMM switch
+	if(this->hardware->dimm.is_new)
+	{
+		this->hardware->dimm.is_new = 0;
+		if(this->hardware->dimm.state) this->hardware->display.write_backlight(this->dimm_pwm);
+		else this->hardware->display.write_backlight(this->brigth_pwm);
+	};
+	
+	// Image processor
+	uint8_t update_img = 0;
+	if(this->next_lock_lvl >= this->act_lock_lvl) update_img = 1;
+	else if(this->hardware->display.is_cycle_end()) update_img = 1;
+	
+	if(update_img)
+	{
+		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+1, this->next_lock_time, 1);
+		}
+		else
+		{
+			this->hardware->display.write(this->act_image);
+			this->act_lock_lvl = 0;
+		}
+	};
+}
+
+/**** Private function definitions ***/
+static uint8_t img_gen_dot10(uint8_t percent)
+{
+	switch(percent)
+	{
+		case 0 ... 5:
+		return 0x01;
+		
+		case 6 ... 15:
+		return 0x03;
+		
+		case 16 ... 25:
+		return 0x02;
+		
+		case 26 ... 35:
+		return 0x06;
+		
+		case 36 ... 45:
+		return 0x04;
+		
+		case 46 ... 55:
+		return 0x0C;
+		
+		case 56 ... 65:
+		return 0x08;
+		
+		case 66 ... 75:
+		return 0x18;
+		
+		case 76 ... 85:
+		return 0x10;
+		
+		case 86 ... 95:
+		return 0x30;
+		
+		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/display.h
+++ b/firmware/src/dccd/display.h
@@ -0,0 +1,55 @@
+#ifndef DCCD_DISPLAY_H_
+#define DCCD_DISPLAY_H_
+
+/**** Includes ****/
+#include <stdint.h>
+#include "dccd_hw.h"
+
+namespace dccd {
+
+/**** Public definitions ****/
+class DccdDisplay
+{
+	public:
+	typedef enum
+	{
+		DOT10 = 0,
+		DOT20 = 1,
+		BAR20 = 2
+	} dspstyle_t;
+	
+	DccdDisplay(void);
+	~DccdDisplay(void);
+	
+	void init(dccd::DccdHw* dccd_hw);
+	void cfg_debounce(uint16_t dbnc_time);
+	
+	uint8_t brigth_pwm;
+	uint8_t dimm_pwm;
+	
+	uint8_t next_image;
+	uint8_t next_lock_lvl;
+	uint16_t next_lock_time;
+	
+	void write(uint8_t image, 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);
+
+	#ifdef TESTING
+	protected:
+	#endif
+	dccd::DccdHw* hardware;
+	
+	uint8_t act_image;
+	uint8_t act_lock_lvl;
+};
+
+/**** Public function declarations ****/
+
+#ifdef TESTING
+#endif
+
+} //namespace
+
+#endif /* DCCD_MODE_BTN_H_ */
--- a/firmware/src/dccd/handbrake.cpp
+++ b/firmware/src/dccd/handbrake.cpp
@@ -0,0 +1,175 @@
+/**** Includes ****/
+#include "../utils/utils.h"
+#include "handbrake.h"
+
+using namespace dccd;
+
+/**** Private definitions ****/
+/**** Private constants ****/
+/**** Private variables ****/
+/**** Private function declarations ****/
+/**** Public function definitions ****/
+dccd::Handbrake::Handbrake(void)
+{
+	return;
+}
+
+dccd::Handbrake::~Handbrake(void)
+{
+	return;
+}
+
+void dccd::Handbrake::init(dccd::DccdHw* dccd_hw)
+{	
+	this->hardware = dccd_hw;
+	
+	this->latch_time_1 = 750;
+	this->latch_time_2 = 1500;
+	this->latch_time_3 = 3000;
+	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)
+{
+	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();
+	
+	if(this->hardware->handbrake.state > 0)
+	{
+		if(this->hardware->handbrake.is_new)
+		{
+			this->hardware->handbrake.is_new = 0;
+			// Note start time
+			this->start_ts = ts_now;
+			this->latch_act = 1;
+		};
+		this->is_active = 1;
+	}
+	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->act_latch_time())
+		{
+			this->latch_act = 0;
+			if(this->hardware->handbrake.state > 0) this->is_active = 1;
+			else this->is_active = 0;
+		};
+	}
+	else
+	{
+		this->is_active = 0;
+	}
+	
+	return this->is_active;
+}
+
+uint8_t dccd::Handbrake::get_mode_int(void)
+{
+	switch(this->mode)
+	{
+		case LATCH0:
+		return 0;
+		
+		case LATCH1:
+		return 1;
+		
+		case LATCH2:
+		return 2;
+		
+		case LATCH3:
+		return 3;
+		
+		default:
+		return 0;
+	}
+}
+
+void dccd::Handbrake::set_mode_int(uint8_t mode_int)
+{
+	switch(mode_int)
+	{
+		case 0:
+		this->mode = LATCH0;
+		break;
+		
+		case 1:
+		this->mode = LATCH1;
+		break;
+		
+		case 2:
+		this->mode = LATCH2;
+		break;
+		
+		case 3:
+		this->mode = LATCH3;
+		break;
+		
+		default:
+		this->mode = LATCH0;
+		break;
+	}
+}
+
+/**** 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;
+	}
+}
--- a/firmware/src/dccd/handbrake.h
+++ b/firmware/src/dccd/handbrake.h
@@ -0,0 +1,57 @@
+#ifndef DCCD_HANDBRAKE_H_
+#define DCCD_HANDBRAKE_H_
+
+/**** Includes ****/
+#include <stdint.h>
+#include "dccd_hw.h"
+
+namespace dccd {
+
+/**** Public definitions ****/
+class Handbrake
+{
+	public:	
+	typedef enum
+	{
+		LATCH0 = 0,
+		LATCH1 = 1,
+		LATCH2 = 2,
+		LATCH3 = 3
+	}hbmode_t;
+		
+	Handbrake(void);
+	~Handbrake(void);
+	
+	void init(dccd::DccdHw* dccd_hw);
+	
+	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);
+	uint8_t get_mode_int(void);
+	void set_mode_int(uint8_t mode_int);
+	
+	#ifdef TESTING
+	protected:
+	#endif
+	dccd::DccdHw* hardware;
+	uint8_t latch_act;
+	uint16_t start_ts;
+	uint16_t act_latch_time(void);
+};
+
+/**** Public function declarations ****/
+
+#ifdef TESTING
+#endif
+
+} //namespace
+
+#endif /* DCCD_HANDBRAKE_H_ */
--- a/firmware/src/dccd/memory.cpp
+++ 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++;
+}
--- a/firmware/src/dccd/memory.h
+++ 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_ */
--- a/firmware/src/dccd/mode.cpp
+++ b/firmware/src/dccd/mode.cpp
@@ -0,0 +1,45 @@
+/**** Includes ****/
+#include "../utils/utils.h"
+#include "mode.h"
+
+using namespace dccd;
+
+/**** Private definitions ****/
+/**** Private constants ****/
+/**** Private variables ****/
+/**** Private function declarations ****/
+/**** Public function definitions ****/
+dccd::ModeBtn::ModeBtn(void)
+{
+	return;
+}
+
+dccd::ModeBtn::~ModeBtn(void)
+{
+	return;
+}
+
+void dccd::ModeBtn::init(dccd::DccdHw* dccd_hw)
+{	
+	this->hardware = dccd_hw;
+	
+	this->btn_repeat_time = 0;
+	this->is_new = 0;
+}
+
+void dccd::ModeBtn::cfg_debounce(uint16_t dbnc_time)
+{
+	this->hardware->btn_mode.dbnc_lim = dbnc_time;
+}
+
+void dccd::ModeBtn::process(void)
+{	
+	if((this->hardware->btn_mode.state==1)&&((this->hardware->btn_mode.is_new)||((this->hardware->btn_mode.time_read() >= this->btn_repeat_time)&&(this->btn_repeat_time!=0))))
+	{
+		this->hardware->btn_mode.time_reset();
+		this->hardware->btn_mode.is_new = 0;
+		this->is_new = 1;
+	};
+}
+
+/**** Private function definitions ***/
--- a/firmware/src/dccd/mode.h
+++ b/firmware/src/dccd/mode.h
@@ -0,0 +1,39 @@
+#ifndef DCCD_MODE_BTN_H_
+#define DCCD_MODE_BTN_H_
+
+/**** Includes ****/
+#include <stdint.h>
+#include "dccd_hw.h"
+
+namespace dccd {
+
+/**** Public definitions ****/
+class ModeBtn
+{
+	public:	
+	ModeBtn(void);
+	~ModeBtn(void);
+	
+	void init(dccd::DccdHw* dccd_hw);
+	void cfg_debounce(uint16_t dbnc_time);
+	
+	uint16_t btn_repeat_time;
+	
+	uint8_t is_new;
+	
+	void process(void);
+
+	#ifdef TESTING
+	protected:
+	#endif
+	dccd::DccdHw* hardware;
+};
+
+/**** Public function declarations ****/
+
+#ifdef TESTING
+#endif
+
+} //namespace
+
+#endif /* DCCD_MODE_BTN_H_ */
--- a/firmware/src/dccd/tps.cpp
+++ b/firmware/src/dccd/tps.cpp
@@ -0,0 +1,158 @@
+/**** Includes ****/
+#include "../utils/utils.h"
+#include "tps.h"
+
+using namespace dccd;
+
+/**** Private definitions ****/
+/**** Private constants ****/
+/**** Private variables ****/
+/**** Private function declarations ****/
+/**** Public function definitions ****/
+dccd::Thtrottle::Thtrottle(void)
+{
+	return;
+}
+
+dccd::Thtrottle::~Thtrottle(void)
+{
+	return;
+}
+
+void dccd::Thtrottle::init(dccd::DccdHw* dccd_hw)
+{	
+	this->hardware = dccd_hw;
+	
+	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;
+	}
+	else
+	{
+		if(this->hardware->pot.last_percent >= this->treshold_on)
+		{
+			this->start_ts = ts_now;
+			this->active = 1;
+			this->is_new = 1;
+		}
+		else this->active = 0;
+	}
+}
+
+uint8_t dccd::Thtrottle::get_mode_int(void)
+{
+	switch(this->mode)
+	{
+		case MODE0:
+		return 0;
+		
+		case MODE1:
+		return 1;
+		
+		case MODE2:
+		return 2;
+		
+		case MODE3:
+		return 3;
+		
+		default:
+		return 0;
+	}
+}
+
+void dccd::Thtrottle::set_mode_int(uint8_t mode_int)
+{
+	switch(mode_int)
+	{
+		case 0:
+		this->mode = MODE0;
+		break;
+		
+		case 1:
+		this->mode = MODE1;
+		break;
+		
+		case 2:
+		this->mode = MODE2;
+		break;
+		
+		case 3:
+		this->mode = MODE3;
+		break;
+		
+		default:
+		this->mode = MODE0;
+		break;
+	}
+}
+
+uint8_t dccd::Thtrottle::is_active(void)
+{
+	if(this->is_timed_out) return 0;
+	else return this->active;
+}
+
+/**** Private function definitions ***/
--- a/firmware/src/dccd/tps.h
+++ b/firmware/src/dccd/tps.h
@@ -0,0 +1,58 @@
+#ifndef DCCD_TPS_H_
+#define DCCD_TPS_H_
+
+/**** Includes ****/
+#include <stdint.h>
+#include "dccd_hw.h"
+
+namespace dccd {
+
+/**** Public definitions ****/
+class Thtrottle
+{
+	public:	
+	typedef enum
+	{
+		MODE0 = 0,
+		MODE1 = 1,
+		MODE2 = 2,
+		MODE3 = 3
+	}tpsmode_t;
+	
+	Thtrottle(void);
+	~Thtrottle(void);
+	
+	void init(dccd::DccdHw* dccd_hw);
+	
+	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);
+	uint8_t get_mode_int(void);
+	void set_mode_int(uint8_t mode_int);
+
+	#ifdef TESTING
+	protected:
+	#endif
+	dccd::DccdHw* hardware;
+	uint16_t start_ts;
+	uint8_t is_timed_out;
+	uint8_t active;
+};
+
+/**** Public function declarations ****/
+
+#ifdef TESTING
+#endif
+
+} //namespace
+
+#endif /* DCCD_TPS_H_ */
--- a/firmware/src/dccd/user_force.cpp
+++ b/firmware/src/dccd/user_force.cpp
@@ -0,0 +1,86 @@
+/**** Includes ****/
+#include "../utils/utils.h"
+#include "user_force.h"
+
+using namespace dccd;
+
+/**** Private definitions ****/
+/**** Private constants ****/
+/**** Private variables ****/
+/**** Private function declarations ****/
+/**** Public function definitions ****/
+dccd::UserForce::UserForce(void)
+{
+	return;
+}
+
+dccd::UserForce::~UserForce(void)
+{
+	return;
+}
+
+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;
+	this->pot_mode = 0;
+	this->btn_force = 0;
+	this->btn_step = 20;
+}
+
+void dccd::UserForce::cfg_debounce(uint16_t dbnc_time)
+{
+	this->hardware->btn_up.dbnc_lim = dbnc_time;
+	this->hardware->btn_down.dbnc_lim = dbnc_time;
+}
+
+void dccd::UserForce::cfg_pot(uint16_t dead_bot, uint16_t dead_top)
+{
+	if(this->pot_mode==0) return;
+	this->hardware->pot.high_deadzone = dead_top;
+	this->hardware->pot.low_deadzone = dead_bot;
+}
+
+void dccd::UserForce::write_force(uint8_t force)
+{
+	if(force > 100) force = 100;
+	this->btn_force = force;
+	this->force = force;
+}
+
+uint8_t dccd::UserForce::process(void)
+{
+	if(this->pot_mode)
+	{
+		this->force = this->hardware->pot.last_percent;
+		return this->force;
+	};
+	
+	if((this->hardware->btn_up.state==1)&&((this->hardware->btn_up.is_new)||((this->hardware->btn_up.time_read() >= this->btn_repeat_time)&&(this->btn_repeat_time!=0))))
+	{
+		this->hardware->btn_up.time_reset();
+		this->hardware->btn_up.is_new = 0;
+		// Increase user force
+		this->btn_force += this->btn_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_repeat_time)&&(this->btn_repeat_time!=0))))
+	{
+		this->hardware->btn_down.time_reset();
+		this->hardware->btn_down.is_new = 0;
+		// Decrease user force
+		this->btn_force -= this->btn_step;
+		if(this->btn_force > 100) this->btn_force = 0;
+		is_new_btn_force = 1;
+	};
+
+	this->force = this->btn_force;
+	return this->force;
+}
+
+/**** Private function definitions ***/
--- a/firmware/src/dccd/user_force.h
+++ b/firmware/src/dccd/user_force.h
@@ -0,0 +1,45 @@
+#ifndef DCCD_USER_FORCE_H_
+#define DCCD_USER_FORCE_H_
+
+/**** Includes ****/
+#include <stdint.h>
+#include "dccd_hw.h"
+
+namespace dccd {
+
+/**** Public definitions ****/
+class UserForce
+{
+	public:	
+	UserForce(void);
+	~UserForce(void);
+	
+	void init(dccd::DccdHw* dccd_hw);
+	void cfg_debounce(uint16_t dbnc_time);
+	void cfg_pot(uint16_t dead_bot, uint16_t dead_top);
+	
+	uint16_t btn_repeat_time;
+	uint8_t pot_mode;
+	uint8_t btn_step;
+	
+	uint8_t force;
+	uint8_t is_new_btn_force;
+	
+	uint8_t process(void);
+	void write_force(uint8_t force);
+	
+	#ifdef TESTING
+	protected:
+	#endif
+	dccd::DccdHw* hardware;
+	uint8_t btn_force;
+};
+
+/**** Public function declarations ****/
+
+#ifdef TESTING
+#endif
+
+} //namespace
+
+#endif /* DCCD_USER_FORCE_H_ */
--- 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;
--- 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,33 +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 = 1;
+	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 = 750;
+	def_cfg.hbrake_latch_time_2 = 1500;
+	def_cfg.hbrake_latch_time_3 = 3000;
+	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 = 1;
+	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 = 0;
+	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;
 	
-	//#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
+	// 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; 
 	}
--- a/firmware/src/uDCCD.cppproj
+++ b/firmware/src/uDCCD.cppproj
@@ -46,7 +46,7 @@
    <com_atmel_avrdbg_tool_atmelice>
      <ToolOptions>
        <InterfaceProperties>
-          <IspClock>249992</IspClock>
+          <IspClock>125000</IspClock>
        </InterfaceProperties>
        <InterfaceName>ISP</InterfaceName>
      </ToolOptions>
@@ -55,7 +55,7 @@
      <ToolName>Atmel-ICE</ToolName>
    </com_atmel_avrdbg_tool_atmelice>
    <avrtoolinterface>ISP</avrtoolinterface>
-    <avrtoolinterfaceclock>249992</avrtoolinterfaceclock>
+    <avrtoolinterfaceclock>125000</avrtoolinterfaceclock>
  </PropertyGroup>
  <PropertyGroup Condition=" '$(Configuration)' == 'Release' ">
    <ToolchainSettings>
@@ -217,6 +217,12 @@
    <Compile Include="bsp\mcu\mcu_hal_r8.cpp">
      <SubType>compile</SubType>
    </Compile>
+    <Compile Include="dccd\brakes.cpp">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="dccd\brakes.h">
+      <SubType>compile</SubType>
+    </Compile>
    <Compile Include="dccd\dccd.cpp">
      <SubType>compile</SubType>
    </Compile>
@@ -229,6 +235,48 @@
    <Compile Include="dccd\dccd_hw.h">
      <SubType>compile</SubType>
    </Compile>
+    <Compile Include="dccd\display.cpp">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="dccd\display.h">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="dccd\coil_reg.cpp">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="dccd\coil_reg.h">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="dccd\handbrake.cpp">
+      <SubType>compile</SubType>
+    </Compile>
+    <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>
+    <Compile Include="dccd\mode.h">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="dccd\tps.cpp">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="dccd\tps.h">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="dccd\user_force.cpp">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="dccd\user_force.h">
+      <SubType>compile</SubType>
+    </Compile>
    <Compile Include="hw\button.cpp">
      <SubType>compile</SubType>
    </Compile>
--- 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;
 }
Author	SHA1	Message	Date
Andis Zīle	f8c09f1a49	Saved work	2024-12-03 09:17:59 +02:00
Andis Zīle	25e6f593d3	Added mode translators	2024-09-17 23:24:01 +03:00
Andis Zīle	e199a2c2be	Saved work	2024-08-20 16:44:35 +03:00
Andis Zīle	657915fa01	Created more helpers	2024-08-02 10:18:09 +03:00