Saved work

.gitignore

@@ -1,3 +1,23 @@
+# Ignore list for Atmel studio files
+# Hidden folder
+.vs/
+
+#Build Directories
+Debug/
+Release/
+
+#Build Results
+*.o
+*.d
+*.eep
+*.elf
+*.hex
+*.map
+*.srec
+
+#User Specific Files
+*.atsuo
+
 # Ignore list for Altium temp files
 History/
 __Previews/
@@ -9,3 +29,4 @@ Project Logs for*
 # Ignore list for Generated output files
 *.step
 OUTPUTS/
+

firmware/src/board/ain.cpp

@@ -0,0 +1,35 @@
+/**** Includes ****/
+#include "../utils/utils.h"
+#include "mcu/mcu_hal.h"
+#include "ain.h"
+
+using namespace board;
+
+/**** Private definitions ****/
+/**** Private constants ****/
+/**** Private variables ****/
+/**** Private function declarations ****/
+
+/**** Public function definitions ****/
+board::AnalogIn::AnalogIn(uint8_t adc_ch)
+{
+	this->adc_ch = adc_ch;
+	this->mul = DEF_AIN_MUL;
+	this->div = DEF_AIN_DIV;
+	this->offset = DEF_AIN_OFFSET;
+	this->last_read = 0;
+}
+
+uint16_t board::AnalogIn::read(void)
+{
+	//Read ADC
+	uint16_t raw = mcu::adc_read(this->adc_ch);
+	
+	//Convert to mV
+	this->last_read = util::convert_muldivoff(raw, this->mul, this->div, this->offset);
+	
+	return this->last_read;
+}
+
+/**** Private function definitions ****/
+

firmware/src/board/ain.h

@@ -0,0 +1,37 @@
+#ifndef ANALOG_IN_H_
+#define ANALOG_IN_H_
+
+/**** Includes ****/
+#include <stdint.h>
+
+namespace board {
+
+/**** Public definitions ****/
+static const uint8_t DEF_AIN_MUL = 215;
+static const uint8_t DEF_AIN_DIV = 44;
+static const int16_t DEF_AIN_OFFSET = 0;
+
+class AnalogIn
+{
+	protected:
+	uint8_t adc_ch;
+	
+	public:
+	AnalogIn(uint8_t adc_ch);
+	
+	uint8_t mul;
+	uint8_t div;
+	int16_t offset;
+	uint16_t last_read;
+	
+	uint16_t read(void);
+};
+
+/**** Public function declarations ****/
+
+#ifdef TESTING
+#endif
+
+} //namespace
+
+#endif /* ANALOG_IN_H_ */

firmware/src/board/din.cpp

@@ -0,0 +1,40 @@
+/**** Includes ****/
+#include "../utils/utils.h"
+#include "mcu/mcu_hal.h"
+#include "din.h"
+
+using namespace board;
+
+/**** Private definitions ****/
+/**** Private constants ****/
+/**** Private variables ****/
+/**** Private function declarations ****/
+
+/**** Public function definitions ****/
+board::DigitalIn::DigitalIn(uint8_t gpio_ch, uint8_t inverted, uint8_t init_value)
+{
+	this->gpio_ch = gpio_ch;
+	this->invert = inverted;
+	
+	if(init_value) this->last_read = DIN_HIGH;
+	else this->last_read = DIN_LOW;
+}
+
+board::DigitalIn::~DigitalIn(void)
+{
+	return;
+}
+
+uint8_t board::DigitalIn::read(void)
+{
+	uint8_t lvl = mcu::gpio_read(this->gpio_ch);
+	
+	if(this->invert) lvl = util::invert(lvl);
+	
+	if(lvl>0) this->last_read = DIN_HIGH;
+	else this->last_read = DIN_LOW;
+	
+	return this->last_read;
+}
+
+/**** Private function definitions ****/

firmware/src/board/din.h

@@ -0,0 +1,35 @@
+#ifndef DIGITAL_INPUT_H_
+#define DIGITAL_INPUT_H_
+
+/**** Includes ****/
+#include <stdint.h>
+
+namespace board {
+
+/**** Public definitions ****/
+const uint8_t DIN_LOW  = 0;
+const uint8_t DIN_HIGH = 1;
+
+class DigitalIn
+{
+	protected:
+	uint8_t gpio_ch;
+	uint8_t invert;
+	
+	public:
+	DigitalIn(uint8_t gpio_ch, uint8_t inverted, uint8_t init_value);
+	~DigitalIn(void);
+	
+	uint8_t last_read;
+	
+	uint8_t read(void);
+};
+
+/**** Public function declarations ****/
+
+#ifdef TESTING
+#endif
+
+} //namespace
+
+#endif /* DIGITAL_INPUT_H_ */

firmware/src/board/dio.cpp

@@ -0,0 +1,34 @@
+/**** Includes ****/
+#include "../utils/utils.h"
+#include "mcu/mcu_hal.h"
+#include "dio.h"
+
+using namespace board;
+
+/**** Private definitions ****/
+/**** Private constants ****/
+/**** Private variables ****/
+/**** Private function declarations ****/
+
+/**** Public function definitions ****/
+board::DigitalIO::DigitalIO(uint8_t gpio_ch, uint8_t init_value) : DigitalIn(gpio_ch, 0, init_value), DigitalOut(gpio_ch, 0)
+{
+	return;
+}
+
+board::DigitalIO::~DigitalIO(void)
+{
+	this->write(DOUT_HIZ);
+}
+
+uint8_t board::DigitalIO::is_io_match(void)
+{
+	if(this->last_set == DOUT_HIZ) return 1;
+	
+	uint8_t read_lvl = this->read();
+	
+	if(read_lvl == (uint8_t)this->last_set) return 1;
+	else return 0;
+}
+
+/**** Private function definitions ****/

firmware/src/board/dio.h

@@ -0,0 +1,32 @@
+#ifndef DIGITAL_IO_H_
+#define DIGITAL_IO_H_
+
+/**** Includes ****/
+#include <stdint.h>
+#include "din.h"
+#include "dout.h"
+
+namespace board {
+
+/**** Public definitions ****/
+const int8_t DIO_LOW  = 0;
+const int8_t DIO_HIGH = 1;
+const int8_t DIO_HIZ   = -1;
+
+class DigitalIO : public DigitalIn, public DigitalOut
+{
+	public:
+	DigitalIO(uint8_t gpio_ch, uint8_t init_value);
+	~DigitalIO(void);
+	
+	uint8_t is_io_match(void);
+};
+
+/**** Public function declarations ****/
+
+#ifdef TESTING
+#endif
+
+} //namespace
+
+#endif /* DIGITAL_IO_H_ */

firmware/src/board/dout.cpp

@@ -0,0 +1,52 @@
+/**** Includes ****/
+#include "../utils/utils.h"
+#include "mcu/mcu_hal.h"
+#include "dout.h"
+
+using namespace board;
+
+/**** Private definitions ****/
+/**** Private constants ****/
+/**** Private variables ****/
+/**** Private function declarations ****/
+
+/**** Public function definitions ****/
+board::DigitalOut::DigitalOut(uint8_t gpio_ch, uint8_t inverted)
+{
+	this->gpio_ch = gpio_ch;
+	this->invert = inverted;
+	this->write(DOUT_HIZ);
+}
+
+board::DigitalOut::~DigitalOut(void)
+{
+	this->write(DOUT_HIZ);
+}
+
+void board::DigitalOut::write(int8_t level)
+{
+	if(level > 0)
+	{
+		this->last_set = DOUT_HIGH;
+		if(this->invert) mcu::gpio_write(this->gpio_ch, mcu::LEVEL_LOW);
+		else mcu::gpio_write(this->gpio_ch, mcu::LEVEL_HIGH);
+	}
+	else if(level == 0)
+	{
+		this->last_set = DOUT_LOW;
+		if(this->invert) mcu::gpio_write(this->gpio_ch, mcu::LEVEL_HIGH);
+		else mcu::gpio_write(this->gpio_ch, mcu::LEVEL_LOW);
+	}
+	else
+	{
+		this->last_set = DOUT_HIZ;
+		mcu::gpio_write(this->gpio_ch, mcu::LEVEL_HIZ);
+	}
+}
+
+int8_t board::DigitalOut::get_set_level(void)
+{
+	return this->last_set;
+}
+
+/**** Private function definitions ****/

firmware/src/board/dout.h

@@ -0,0 +1,36 @@
+#ifndef DIGITAL_OUTPUT_H_
+#define DIGITAL_OUTPUT_H_
+
+/**** Includes ****/
+#include <stdint.h>
+
+namespace board {
+
+/**** Public definitions ****/
+const int8_t DOUT_LOW  = 0;
+const int8_t DOUT_HIGH = 1;
+const int8_t DOUT_HIZ = -1;
+
+class DigitalOut
+{
+	protected:
+	uint8_t gpio_ch;
+	uint8_t invert;
+	int8_t last_set;
+	
+	public:
+	DigitalOut(uint8_t gpio_ch, uint8_t inverted);
+	~DigitalOut(void);
+	
+	void write(int8_t level);
+	int8_t get_set_level(void);
+};
+
+/**** Public function declarations ****/
+
+#ifdef TESTING
+#endif
+
+} //namespace
+
+#endif /* DIGITAL_OUTPUT_H_ */

firmware/src/board/halfbridge.cpp

@@ -0,0 +1,74 @@
+/**** Includes ****/
+#include "../utils/utils.h"
+#include "mcu/mcu_hal.h"
+#include "halfbridge.h"
+
+using namespace board;
+
+/**** Private definitions ****/
+/**** Private constants ****/
+/**** Private variables ****/
+/**** Private function declarations ****/
+
+/**** Public function definitions ****/
+board::Hafbridge::Hafbridge(uint8_t hs_pwm_ch, uint8_t ls_gpio_ch, uint8_t max_dc)
+{
+	this->pwm_ch  = hs_pwm_ch;	
+	this->gpio_ch = ls_gpio_ch;
+	
+    if(max_dc>100) max_dc = 100;
+	
+	this->max_dc = util::percent_to_16b(max_dc);
+
+	this->disable();
+}
+
+board::Hafbridge::~Hafbridge(void)
+{
+	this->last_duty = 0;
+	this->disable();
+}
+
+void board::Hafbridge::write(uint16_t dividend)
+{
+	// Limit duty
+	if(dividend > this->max_dc) dividend = this->max_dc;
+	this->last_duty = dividend;
+	
+	if(this->enabled == 0) return;
+	
+	// Set PWM
+	mcu::pwm_write(this->pwm_ch, dividend);
+}
+
+void board::Hafbridge::write(uint8_t percent)
+{
+	// Convert to dividend/0xFFFF
+	this->write(util::percent_to_16b(percent));
+}
+
+void board::Hafbridge::enable(void)
+{
+	mcu::gpio_write(this->gpio_ch, mcu::LEVEL_HIGH);
+	this->enabled = 1;
+	this->write(this->last_duty);
+}
+
+void board::Hafbridge::disable(void)
+{
+	mcu::pwm_write(this->pwm_ch, 0);
+	mcu::gpio_write(this->gpio_ch, mcu::LEVEL_LOW);
+	this->enabled = 0;
+}
+
+uint8_t board::Hafbridge::get_set_duty(void)
+{
+	return this->last_duty;
+}
+
+uint8_t board::Hafbridge::is_enabled(void)
+{
+	return this->enabled;
+}
+
+/**** Private function definitions ****/

firmware/src/board/halfbridge.h

@@ -0,0 +1,38 @@
+#ifndef HALFBRIDGE_H_
+#define HALFBRIDGE_H_
+
+/**** Includes ****/
+#include <stdint.h>
+
+namespace board {
+
+/**** Public definitions ****/
+class Hafbridge
+{
+	protected:
+	uint8_t pwm_ch;
+	uint8_t gpio_ch;
+	uint16_t last_duty;
+	uint8_t enabled;
+	uint16_t max_dc;
+	
+	public:
+	Hafbridge(uint8_t hs_pwm_ch, uint8_t ls_gpio_ch, uint8_t max_dc);
+	~Hafbridge(void);
+	
+	void write(uint16_t dividend);
+	void write(uint8_t percent);
+	void enable(void);
+	void disable(void);
+	uint8_t get_set_duty(void);
+	uint8_t is_enabled(void);
+};
+
+/**** Public function declarations ****/
+
+#ifdef TESTING
+#endif
+
+} //namespace
+
+#endif /* HALFBRIDGE_H_ */

firmware/src/board/mcu/mcu_hal.h

@@ -0,0 +1,98 @@
+#ifndef MCU_HAL_H_
+#define MCU_HAL_H_
+
+/**** Includes ****/
+#include <stdint.h>
+
+namespace mcu {
+
+/**** Public definitions ****/
+/*
+*/
+
+const uint8_t GPIO0 = 0;	//PC5 Mode
+const uint8_t GPIO1 = 1;	//PC4 Pot  
+const uint8_t GPIO2 = 2;	//PE1 Down
+const uint8_t GPIO3 = 3;	//PE3 Up
+const uint8_t GPIO4 = 4;	//PD7 Dimm
+const uint8_t GPIO5 = 5;	//PB7 Brakes
+const uint8_t GPIO6 = 6;	//PB6 Handbrake
+const uint8_t GPIO7 = 7;	//PB5 Handbrake pull
+const uint8_t GPIO8 = 8;	//PD6 Speed pull
+const uint8_t GPIO9 = 9;	//PD0 LED0
+const uint8_t GPIO10 = 10;	//PD1 LED1
+const uint8_t GPIO11 = 11;	//PD2 LED2
+const uint8_t GPIO12 = 12;	//PD3 LED3
+const uint8_t GPIO13 = 13;	//PD4 LED4
+const uint8_t GPIO14 = 14;	//PD5 LED5
+const uint8_t GPIO15 = 15;	//PB0 DCCD Enable
+const uint8_t GPIO16 = 16;	//PB1 DCCD PWM
+const uint8_t GPIO17 = 17;	//PB2 LED PWM
+
+const uint8_t LEVEL_LOW  = 0;
+const uint8_t LEVEL_HIGH = 1;
+const int8_t LEVEL_HIZ   = -1;
+
+const uint8_t ADC0 = 0;	//Output current
+const uint8_t ADC1 = 1;	//Output voltage
+const uint8_t ADC2 = 2;	//Battery voltage
+const uint8_t ADC3 = 3;	//Battery current
+const uint8_t ADC4 = 4;	//Potentiometer
+const uint8_t ADC5 = 5;	//Mode
+const uint8_t ADC8 = 8;	//MCU temperature
+const uint8_t ADC14 = 14;	//MCU internal reference
+const uint8_t ADC15 = 15;	//MCU ground
+
+const uint8_t PWM0 = 0;	//DCCD
+const uint8_t PWM1 = 1;	//LED
+
+//ADC definitions
+typedef enum {
+	ADC_DIV2 = 0x01,
+	ADC_DIV4 = 0x02,
+	ADC_DIV8 = 0x03,
+	ADC_DIV16 = 0x04,
+	ADC_DIV32 = 0x05,
+	ADC_DIV64 = 0x06,
+	ADC_DIV128 = 0x07
+} adcClkDiv_t;
+
+//Timer definitions
+typedef enum {
+	TIM_DIV1 = 0x01,
+	TIM_DIV8 = 0x02,
+	TIM_DIV64 = 0x03,
+	TIM_DIV256 = 0x04,
+	TIM_DIV1024 = 0x05
+} timerClkDiv_t;
+
+typedef struct {
+	adcClkDiv_t adc_clk;
+	timerClkDiv_t pwm_clk;
+	uint16_t pwm_top;
+	uint8_t pwm_ch1_en;
+} startupCfg_t;
+
+/**** Public function declarations ****/
+void startup(startupCfg_t* hwCfg);
+
+uint8_t gpio_read(uint8_t ch);
+void gpio_write(uint8_t ch, int8_t lvl);
+void gpio_write_pull(uint8_t ch, int8_t lvl);
+
+uint16_t adc_read(uint8_t ch);
+
+void pwm_write(uint8_t ch, uint16_t dc);
+uint16_t pwm_read(uint8_t ch);
+
+uint8_t eeprom_read8b(uint16_t address);
+uint16_t eeprom_read16b(uint16_t address);
+uint32_t eeprom_read32b(uint16_t address);
+
+void eeprom_write8b(uint16_t address, uint8_t value);
+void eeprom_write16b(uint16_t address, uint16_t value);
+void eeprom_write32b(uint16_t address, uint32_t value);
+
+} //namespace
+
+#endif /* MCU_HAL_H_ */

firmware/src/board/mcu/mcu_hal_r8.cpp

@@ -0,0 +1,464 @@
+/**** Includes ****/
+#include <avr/io.h>
+#include <avr/eeprom.h>
+#include "mcu_hal.h"
+
+using namespace mcu;
+
+/**** Private definitions ****/
+/**** Private constants ****/
+/**** Private variables ****/
+/**** Private function declarations ****/
+static uint8_t gpio_read_level(uint8_t pin_reg, uint8_t mask);
+static void pwm_write_ocx(uint8_t ch, uint16_t value);
+static uint16_t pwm_read_ocx(uint8_t ch);
+
+/**** Public function definitions ****/
+void mcu::startup(startupCfg_t* hwCfg)
+{
+	// Fail-safe GPIO init
+	PORTB = 0xF8;  // Set PORTB pull-ups
+	DDRB = 0x00;   // Set all as inputs
+	
+	PORTC = 0x40;  // Set PORTC pull-ups
+	DDRC = 0x00;   // Set all as inputs
+	
+	PORTD = 0x80;  // Set PORTD pull-ups
+	DDRD = 0x00;   // Set all as inputs
+	
+	PORTE = 0x0A;  // Set PORTE pull-ups
+	DDRE = 0x00;   // Set all as inputs
+	
+	// Half-bridge related pins
+	PORTB &= ~0x03; //Set low
+	DDRB |= 0x03;   //Set as output
+	
+	// Common OD PWM pin
+	if(hwCfg->pwm_ch1_en) PORTB &= ~0x04; //Set low
+	else PORTB |= 0x04;                   //Set high
+	DDRB |= 0x04;                         //Set as output
+	
+	// OD control pins
+	PORTD &= ~0x3F; //Set low (off)
+	DDRD |= 0x3F;   //Set as outputs
+	
+	// Handbrake pull-up pin
+	PORTB |= 0x20;  //Set high
+	DDRB |= 0x20;   //Set as output
+	
+	// Handbrake and brakes pins
+	PORTB |= 0xC0;  //Set pull-up on
+	DDRB &= ~0xC0;   //Set as inputs
+	
+	// Dimm
+	PORTD |= 0x80;  //Set pull-up on
+	DDRD &= ~0x80;   //Set as input
+	
+	// Up and Down
+	PORTE |= 0x0A;  //Set pull-up on
+	DDRE &= ~0x0A;   //Set as inputs
+	
+	// Internal ADC inputs
+	PORTC &= ~0x0F; //Pull-up off
+	DDRC &= ~0x0F;  //Set as inputs
+	
+	// Potentiometer & Mode
+	PORTC &= ~0x30; //Pull-up off
+	DDRC &= ~0x30;  //Set as inputs
+	
+	//ADC configuration
+	PRR0 &= ~0x01; //Enable ADC power
+	DIDR0 |= 0x0F; //Disable digital inputs, ADC0-ADC3
+	
+	ADMUX = 0x40;  //Set AVCC reference, Right adjust
+	ADCSRA = 0x00; //ADC Disabled, Single conversion, no IT
+	ADCSRA |= (uint8_t)hwCfg->adc_clk;
+	ADCSRB = 0x00; //no trigger input
+	
+	ADCSRA |= 0x80;  //Enable ADC
+	
+	//DCCD and LED PWM configuration
+	PRR0 &= ~0x80;  //Enable Timer1 power
+	TCCR1A = 0xC2; //Connect OC1A, inverted mode
+	if(hwCfg->pwm_ch1_en) TCCR1A |= 0x30; //Connect OC1B, inverted mode
+	TCCR1B = 0x18; //PWM, Phase & Frequency Correct ICR1 top, no clock, WGM:0xE
+	TCCR1C = 0x00;
+	TCNT1 = 0x0000;
+	OCR1A = 0xFFFF;
+	OCR1B = 0xFFFF;
+	ICR1 = hwCfg->pwm_top;
+	TIMSK1 = 0x00;  //No interrupts
+	TIFR1 = 0x00;	//Clear all flags
+	
+	uint8_t tim1_prescaler = (uint8_t)hwCfg->pwm_clk;
+	TCCR1B |= tim1_prescaler;  //Enable timer
+}
+
+// ADC Interface functions 
+uint16_t mcu::adc_read(uint8_t ch)
+{
+	//check if ADC is enabled
+	if(!(ADCSRA&0x80)) return 0xFFFF;
+
+	//Safe guard mux
+	if(ch > 15) return 0xFFFF;
+	// Not available channels
+	if((ch > 8) && (ch<14)) return 0xFFFF;
+	
+	ADMUX &= ~0x0F;
+	ADMUX |= ch;
+	ADCSRA |= 0x40;
+	while(ADCSRA&0x40); //wait to finish
+	return ADC;
+}
+
+// PWM Timer Interface functions
+void mcu::pwm_write(uint8_t ch, uint16_t dc)
+{
+	dc = 0xFFFF - dc;
+	
+	// Calculate value as % of TOP
+	uint32_t top = (uint32_t)ICR1;
+	uint32_t temp = (uint32_t)dc * top;
+	temp = temp/0x0000FFFF;
+	
+	//Limit temp
+	if(temp>0x0000FFFF) temp = 0x0000FFFF;
+	uint16_t ocrx = (uint16_t)temp;
+	
+	// Write register
+	pwm_write_ocx(ch, ocrx);
+}
+
+uint16_t mcu::pwm_read(uint8_t ch)
+{
+	uint16_t ocrx = pwm_read_ocx(ch);
+	
+	// Check easy answers
+	if(ocrx == 0) return 0;
+	if(ocrx >= ICR1) return 0xFFFF;
+	
+	// Calculate
+	uint32_t top = (uint32_t)ICR1;
+	uint32_t temp = (uint32_t)ocrx * 0xFFFF;
+	temp = temp/top;
+	
+	//Limit temp
+	if(temp>0x0000FFFF) return 0xFFFF;
+	return (uint16_t)temp;
+}
+
+uint8_t mcu::gpio_read(uint8_t ch)
+{
+	switch(ch)
+	{
+		case GPIO0:  // Mode DIN1
+		return gpio_read_level(PINC,0x20);
+		
+		case GPIO1:  // Pot DIN2
+		return gpio_read_level(PINC,0x10);
+		
+		case GPIO2:  // Down DIN3
+		return gpio_read_level(PINE,0x02);
+		
+		case GPIO3:  // Up DIN4
+		return gpio_read_level(PINE,0x08);
+		
+		case GPIO4:  // Dimm DIN5
+		return gpio_read_level(PIND,0x80);
+		
+		case GPIO5:  // Brakes DIN6
+		return gpio_read_level(PINB,0x80);
+		
+		case GPIO6:  // Handbrake DIN7
+		return gpio_read_level(PINB,0x40);
+		
+		case GPIO7:  // Handbrake pull DIN8
+		return gpio_read_level(PINB,0x20);
+		
+		case GPIO8:  // Speed-pull
+		return gpio_read_level(PIND,0x40);
+		
+		case GPIO9:  // LED 0
+		return gpio_read_level(PIND,0x01);
+		
+		case GPIO10:  // LED 1
+		return gpio_read_level(PIND,0x02);
+		
+		case GPIO11:  // LED 2
+		return gpio_read_level(PIND,0x04);
+		
+		case GPIO12:  // LED 3
+		return gpio_read_level(PIND,0x08);
+		
+		case GPIO13:  // LED 4
+		return gpio_read_level(PIND,0x10);
+		
+		case GPIO14:  // LED 5
+		return gpio_read_level(PIND,0x20);
+		
+		case GPIO15:  // DCCD Enable
+		return gpio_read_level(PINB,0x01);	
+		
+		case GPIO16:  // DCCD PWM
+		return gpio_read_level(PINB,0x02);
+		
+		case GPIO17:  // LED PWM
+		return gpio_read_level(PINB,0x04);	
+		
+		default:
+		return 0;
+	}
+}
+
+void mcu::gpio_write(uint8_t ch, int8_t lvl)
+{
+	switch(ch)
+	{		
+		case GPIO0:  // Mode DIN1
+		if(lvl>0)
+		{
+			PORTC |= 0x20;
+			DDRC |= 0x20;
+		}
+		else if(lvl<0)
+		{
+			DDRC &= ~0x20;
+			PORTC &= ~0x20;
+		}
+		else
+		{
+			PORTC &= ~0x20;
+			DDRC |= 0x20;
+		}
+		return;
+		
+		case GPIO1:  // Pot DIN2
+		if(lvl>0)
+		{
+			PORTC |= 0x10;
+			DDRC |= 0x10;
+		}
+		else if(lvl<0)
+		{
+			DDRC &= ~0x10;
+			PORTC &= ~0x10;
+		}
+		else
+		{
+			PORTC &= ~0x10;
+			DDRC |= 0x10;
+		}
+		return;
+		
+		case GPIO2:  // Down DIN3
+		if(lvl>0)
+		{
+			PORTE |= 0x02;
+			DDRE |= 0x02;
+		}
+		else if(lvl<0)
+		{
+			DDRE &= ~0x02;
+			PORTE &= ~0x02;
+		}
+		else
+		{
+			PORTE &= ~0x02;
+			DDRE |= 0x02;
+		}
+		return;
+		
+		case GPIO3:  // Up DIN4
+		if(lvl>0)
+		{
+			PORTE |= 0x08;
+			DDRE |= 0x08;
+		}
+		else if(lvl<0)
+		{
+			DDRE &= ~0x08;
+			PORTE &= ~0x08;
+		}
+		else
+		{
+			PORTE &= ~0x08;
+			DDRE |= 0x08;
+		}
+		return;
+		
+		case GPIO7:  // Handbrake pull DIN
+		if(lvl>0)
+		{
+			PORTB |= 0x20;
+			DDRB |= 0x20;
+		}
+		else if(lvl<0)
+		{
+			DDRB &= ~0x20;
+			PORTB &= ~0x20;
+		}
+		else
+		{
+			PORTB &= ~0x20;
+			DDRB |= 0x20;
+		}
+		return;
+		
+		case GPIO8:  // Speed-pull
+		if(lvl>0) PORTD |= 0x40;
+		else PORTD &= ~0x40;
+		return;
+		
+		case GPIO9:  // LED 0
+		if(lvl>0) PORTD |= 0x01;
+		else PORTD &= ~0x01;
+		return;
+		
+		case GPIO10:  // LED 1
+		if(lvl>0) PORTD |= 0x02;
+		else PORTD &= ~0x02;
+		return;
+		
+		case GPIO11:  // LED 2
+		if(lvl>0) PORTD |= 0x04;
+		else PORTD &= ~0x04;
+		return;
+		
+		case GPIO12:  // LED 3
+		if(lvl>0) PORTD |= 0x08;
+		else PORTD &= ~0x08;
+		return;
+		
+		case GPIO13:  // LED 4
+		if(lvl>0) PORTD |= 0x10;
+		else PORTD &= ~0x10;
+		return;
+		
+		case GPIO14:  // LED 5
+		if(lvl>0) PORTD |= 0x20;
+		else PORTD &= ~0x20;
+		return;
+		
+		case GPIO15:  // DCCD Enable
+		if(lvl>0) PORTB |= 0x01;
+		else PORTB &= ~0x01;
+		return;
+		
+		default:
+		return;
+	}
+}
+
+void mcu::gpio_write_pull(uint8_t ch, int8_t lvl)
+{
+	switch(ch)
+	{
+		case GPIO0:  // Mode DIN1
+		if(lvl>0) PORTC |= 0x20;
+		else PORTC &= ~0x20;
+		return;
+		
+		case GPIO1:  // Pot DIN2
+		if(lvl>0) PORTC |= 0x10;
+		else PORTC &= ~0x10;
+		return;
+		
+		case GPIO2:  // Down DIN3
+		if(lvl>0) PORTE |= 0x02;
+		else PORTE &= ~0x02;
+		return;
+		
+		case GPIO3:  // Up DIN4
+		if(lvl>0) PORTE |= 0x08;
+		else PORTE &= ~0x08;
+		return;
+		
+		case GPIO4:  // Dimm
+		if(lvl>0) PORTD |= 0x80;
+		else PORTD &= ~0x80;
+		return;
+		
+		case GPIO5:  // Brakes
+		if(lvl>0) PORTB |= 0x80;
+		else PORTB &= ~0x80;
+		return;
+		
+		case GPIO6:  // Handbrake
+		if(lvl>0) PORTB |= 0x40;
+		else PORTB &= ~0x40;
+		return;
+		
+		default:
+		return;
+	}
+}
+
+uint8_t mcu::eeprom_read8b(uint16_t address)
+{
+	return eeprom_read_byte((uint8_t*)address);
+}
+
+uint16_t mcu::eeprom_read16b(uint16_t address)
+{
+	return eeprom_read_word((uint16_t*)address);
+}
+
+uint32_t mcu::eeprom_read32b(uint16_t address)
+{
+	return eeprom_read_dword((uint32_t*)address);
+}
+
+void mcu::eeprom_write8b(uint16_t address, uint8_t value)
+{
+	eeprom_write_byte((uint8_t*)address, value);
+}
+
+void mcu::eeprom_write16b(uint16_t address, uint16_t value)
+{
+	eeprom_write_word((uint16_t*)address, value);
+}
+
+void mcu::eeprom_write32b(uint16_t address, uint32_t value)
+{
+	eeprom_write_dword((uint32_t*)address, value);
+}
+
+/**** Private function definitions ****/
+static uint8_t gpio_read_level(uint8_t pin_reg, uint8_t mask)
+{
+	if(pin_reg&mask) return LEVEL_HIGH;
+	else return LEVEL_LOW;
+}
+
+static void pwm_write_ocx(uint8_t ch, uint16_t value)
+{
+	switch(ch)
+	{
+		case PWM0:
+		OCR1A = value;
+		return;
+		
+		case PWM1:
+		OCR1B = value;
+		return;
+		
+		default:
+		return;
+	}
+}
+
+static uint16_t pwm_read_ocx(uint8_t ch)
+{
+	switch(ch)
+	{
+		case PWM0:
+		return OCR1A;
+		
+		case PWM1:
+		return OCR1B ;
+		
+		default:
+		return 0x0000;
+	}
+}

firmware/src/board/pwm.cpp

@@ -0,0 +1,40 @@
+/**** Includes ****/
+#include "../utils/utils.h"
+#include "mcu/mcu_hal.h"
+#include "pwm.h"
+
+using namespace board;
+
+/**** Private definitions ****/
+/**** Private constants ****/
+/**** Private variables ****/
+/**** Private function declarations ****/
+
+/**** Public function definitions ****/
+board::PWMout::PWMout(uint8_t pwm_ch)
+{
+	this->pwm_ch = pwm_ch;
+	this->write(0);
+}
+
+board::PWMout::~PWMout(void)
+{
+	this->write(0);
+}
+
+void board::PWMout::write(uint8_t duty)
+{
+	// Convert percent to 16b duty cycle
+	uint16_t dc = util::percent_to_16b(duty);
+	
+	// Set PWM
+	mcu::pwm_write(this->pwm_ch, dc);
+	this->last_duty = duty;
+}
+
+uint8_t board::PWMout::get_set_duty(void)
+{
+	return this->last_duty;
+}
+
+/**** Private function definitions ****/

firmware/src/board/pwm.h

@@ -0,0 +1,31 @@
+#ifndef PWM_H_
+#define PWM_H_
+
+/**** Includes ****/
+#include <stdint.h>
+
+namespace board {
+
+/**** Public definitions ****/
+class PWMout
+{
+	protected:
+	uint8_t pwm_ch;
+	uint8_t last_duty;
+	
+	public:
+	PWMout(uint8_t pwm_ch);
+	~PWMout(void);
+	
+	void write(uint8_t duty);
+	uint8_t get_set_duty(void);
+};
+
+/**** Public function declarations ****/
+
+#ifdef TESTING
+#endif
+
+} //namespace
+
+#endif /* PWM_H_ */

firmware/src/hw/button.cpp

@@ -0,0 +1,85 @@
+/**** Includes ****/
+#include "../utils/utils.h"
+#include "button.h"
+
+using namespace hw;
+
+/**** Private definitions ****/
+/**** Private constants ****/
+/**** Private variables ****/
+/**** Private function declarations ****/
+
+/**** Public function definitions ****/
+hw::Button::Button(board::DigitalIn* din_ch, uint8_t act_lvl, uint8_t dbnc_lim, uint8_t init_state)
+{
+	this->din_ch = din_ch;
+	
+	if(act_lvl) this->act_lvl = board::DIN_HIGH;
+	else this->act_lvl = board::DIN_LOW;
+	
+	this->dbnc_cnter = 0;
+	this->dbnc_lim = dbnc_lim;
+	
+	if(init_state) this->state = BUTTON_ON;
+	else this->state = BUTTON_OFF;
+	
+	this->time = 0;
+	this->is_new = 0;
+}
+
+hw::Button::~Button(void)
+{
+	return;
+}
+
+uint8_t hw::Button::read(void)
+{
+	// Read din level
+	uint8_t lvl = this->din_ch->read();
+	
+	// Increase state counter
+	this->time = util::sat_add(this->time, 1);
+	
+	// Determine next state
+	uint8_t next_state = BUTTON_OFF;
+	if(lvl==this->act_lvl) next_state = BUTTON_ON;
+	
+	// Advance debounce sample counter
+	if(next_state != this->state) this->dbnc_cnter++;
+	else this->dbnc_cnter = 0;
+	
+	// Check for debounce end
+	if(this->dbnc_cnter < this->dbnc_lim) return this->state;
+	
+	// Debounce end. Apply new state.
+	this->state = next_state;
+	this->time = 0;
+	this->is_new = 1;
+	this->dbnc_cnter = 0;
+	
+	return this->state;
+}
+
+uint8_t hw::Button::force_read(void)
+{
+	// Read din level
+	uint8_t lvl = this->din_ch->read();
+	
+	// Cancels active debounce
+	this->dbnc_cnter = 0;
+	
+	// Determine next state
+	uint8_t next_state = BUTTON_OFF;
+	if(lvl==this->act_lvl) next_state = BUTTON_ON;
+	
+	if(next_state != this->state)
+	{
+		this->state = next_state;
+		this->time = 0;
+		this->is_new = 1;
+	};
+	
+	return this->state;
+}
+
+/**** Private function definitions ****/

firmware/src/hw/button.h

@@ -0,0 +1,41 @@
+#ifndef BUTTON_H_
+#define BUTTON_H_
+
+/**** Includes ****/
+#include <stdint.h>
+#include "../board/din.h"
+
+namespace hw {
+
+/**** Public definitions ****/
+const uint8_t BUTTON_OFF  = 0;
+const uint8_t BUTTON_ON = 1;
+
+class Button
+{
+	protected:
+	board::DigitalIn* din_ch;
+	uint8_t act_lvl;
+	uint8_t dbnc_cnter;
+	
+	public:
+	Button(board::DigitalIn* din_ch, uint8_t act_lvl, uint8_t dbnc_lim, uint8_t init_state);
+	~Button(void);
+	
+	uint8_t state;
+	uint16_t time;
+	uint8_t dbnc_lim;
+	uint8_t is_new;
+	
+	uint8_t read(void);
+	uint8_t force_read(void);
+};
+
+/**** Public function declarations ****/
+
+#ifdef TESTING
+#endif
+
+} //namespace
+
+#endif /* BUTTON_H_ */

firmware/src/hw/cv_otput.cpp

@@ -0,0 +1,52 @@
+/**** Includes ****/
+#include "../utils/utils.h"
+#include "cv_output.h"
+
+using namespace hw;
+
+/**** Private definitions ****/
+/**** Private constants ****/
+/**** Private variables ****/
+/**** Private function declarations ****/
+
+/**** Public function definitions ****/
+hw::CVoutput::CVoutput(board::Hafbridge* hbridge)
+{
+	this->hbridge = hbridge;
+	this->target = 0;
+	this->min_out = 0;
+	this->hbridge->disable();
+}
+
+hw::CVoutput::~CVoutput(void)
+{
+	this->hbridge->write((uint16_t)0);
+	this->hbridge->disable();
+	return;
+}
+
+void hw::CVoutput::update(uint16_t supply_mv)
+{
+	// Check target	
+	if((this->target < this->min_out)&&(this->target > 0)) this->target = this->min_out;
+	
+	// Set output
+	this->hbridge->write(util::sat_ratio(this->target, supply_mv));
+}
+
+void hw::CVoutput::enable(void)
+{
+	this->hbridge->enable();
+}
+
+void hw::CVoutput::disable(void)
+{
+	this->hbridge->disable();	
+}
+
+uint8_t hw::CVoutput::is_enabled(void)
+{
+	return this->hbridge->is_enabled();
+}
+
+/**** Private function definitions ****/

firmware/src/hw/cv_output.h

@@ -0,0 +1,37 @@
+#ifndef CONST_VOLTAGE_OUTPUT_H_
+#define CONST_VOLTAGE_OUTPUT_H_
+
+/**** Includes ****/
+#include <stdint.h>
+#include "../board/halfbridge.h"
+
+namespace hw {
+
+/**** Public definitions ****/
+
+class CVoutput
+{
+	protected:
+	board::Hafbridge* hbridge;
+	
+	public:
+	CVoutput(board::Hafbridge* hbridge);
+	~CVoutput(void);
+	
+	uint16_t target;
+	uint16_t min_out;
+	
+	void update(uint16_t supply_mv);
+	void enable(void);
+	void disable(void);
+	uint8_t is_enabled(void);
+};
+
+/**** Public function declarations ****/
+
+#ifdef TESTING
+#endif
+
+} //namespace
+
+#endif /* CONST_VOLTAGE_OUTPUT_H_ */

firmware/src/hw/display_led.cpp

@@ -0,0 +1,180 @@
+/**** Includes ****/
+#include "../utils/utils.h"
+#include "display_led.h"
+
+using namespace hw;
+
+/**** Private definitions ****/
+/**** Private constants ****/
+/**** Private variables ****/
+/**** Private function declarations ****/
+static uint8_t img_gen_dot10(uint8_t percent);
+static uint8_t img_gen_dot20(uint8_t percent);
+static uint8_t img_gen_bar(uint8_t percent);
+
+/**** Public function definitions ****/
+hw::DisplayLed::DisplayLed(board::DigitalOut* led0, board::DigitalOut* led1, board::DigitalOut* led2, board::DigitalOut* led3, board::DigitalOut* led4, board::DigitalOut* led5, board::PWMout* common)
+{
+	this->led0 = led0;
+	this->led1 = led1;
+	this->led2 = led2;
+	this->led3 = led3;
+	this->led4 = led4;
+	this->led5 = led5;
+	this->common = common;
+	
+	this->led0->write(0);
+	this->led1->write(0);
+	this->led2->write(0);
+	this->led3->write(0);
+	this->led4->write(0);
+	this->led5->write(0);
+	this->common->write(0);
+}
+
+hw::DisplayLed::~DisplayLed(void)
+{
+	this->led0->write(0);
+	this->led1->write(0);
+	this->led2->write(0);
+	this->led3->write(0);
+	this->led4->write(0);
+	this->led5->write(0);
+	this->common->write(0);
+}
+
+void hw::DisplayLed::show_percent(uint8_t percent, style_t style)
+{
+	uint8_t image = 0x00;
+	
+	switch(style)
+	{
+		case LED_DSP_BAR:
+		image = img_gen_bar(percent);
+		break;
+		
+		case LED_DSP_DOT10:
+		image = img_gen_dot10(percent);
+		break;
+		
+		default:
+		image = img_gen_dot20(percent);
+		break;
+	}
+	
+	this->write(image);
+}
+
+void hw::DisplayLed::write(uint8_t image)
+{
+	if(image&0x01) this->led0->write(1);
+	else this->led0->write(0);
+	
+	if(image&0x02) this->led1->write(1);
+	else this->led1->write(0);
+	
+	if(image&0x04) this->led2->write(1);
+	else this->led2->write(0);
+	
+	if(image&0x08) this->led3->write(1);
+	else this->led3->write(0);
+	
+	if(image&0x10) this->led4->write(1);
+	else this->led4->write(0);
+	
+	if(image&0x20) this->led5->write(1);
+	else this->led5->write(0);
+}
+
+void hw::DisplayLed::set_brigthness(uint8_t percent)
+{
+	this->common->write(percent);
+}
+
+/**** Private function definitions ****/
+static uint8_t img_gen_dot10(uint8_t percent)
+{
+	switch(percent)
+	{
+		case 0 ... 5:
+		return 0x01;
+		
+		case 6 ... 15:
+		return 0x03;
+		
+		case 16 ... 25:
+		return 0x02;
+		
+		case 26 ... 35:
+		return 0x06;
+		
+		case 36 ... 45:
+		return 0x04;
+		
+		case 46 ... 55:
+		return 0x0C;
+		
+		case 56 ... 65:
+		return 0x08;
+		
+		case 66 ... 75:
+		return 0x18;
+		
+		case 76 ... 85:
+		return 0x10;
+		
+		case 86 ... 95:
+		return 0x30;
+		
+		default:
+		return 0x20;
+	}
+}
+
+static uint8_t img_gen_dot20(uint8_t percent)
+{
+	switch(percent)
+	{
+		case 0 ... 10:
+		return 0x01;
+		
+		case 11 ... 30:
+		return 0x02;
+		
+		case 31 ... 50:
+		return 0x04;
+		
+		case 51 ... 70:
+		return 0x08;
+		
+		case 71 ... 90:
+		return 0x10;
+		
+		default:
+		return 0x20;
+	}
+}
+
+static uint8_t img_gen_bar(uint8_t percent)
+{
+	switch(percent)
+	{
+		case 0 ... 10:
+		return 0x01;
+		
+		case 11 ... 30:
+		return 0x03;
+		
+		case 31 ... 50:
+		return 0x07;
+		
+		case 51 ... 70:
+		return 0x0F;
+		
+		case 71 ... 90:
+		return 0x1F;
+		
+		default:
+		return 0x3F;
+	}
+}

firmware/src/hw/display_led.h

@@ -0,0 +1,47 @@
+#ifndef DISPLAY_LED_H_
+#define DISPLAY_LED_H_
+
+/**** Includes ****/
+#include <stdint.h>
+#include "../board/dout.h"
+#include "../board/pwm.h"
+
+namespace hw {
+
+/**** Public definitions ****/
+
+class DisplayLed
+{
+	protected:
+	board::DigitalOut* led0;
+	board::DigitalOut* led1;
+	board::DigitalOut* led2;
+	board::DigitalOut* led3;
+	board::DigitalOut* led4;
+	board::DigitalOut* led5;
+	board::PWMout* common;
+	
+	public:
+	typedef enum {
+		LED_DSP_DOT20,
+		LED_DSP_DOT10,
+		LED_DSP_BAR
+	} style_t;
+	
+	DisplayLed(board::DigitalOut* led0, board::DigitalOut* led1, board::DigitalOut* led2, board::DigitalOut* led3, board::DigitalOut* led4, board::DigitalOut* led5, board::PWMout* common);
+	~DisplayLed(void);
+	
+	void show_percent(uint8_t percent, style_t style);
+	void write(uint8_t image);
+	
+	void set_brigthness(uint8_t percent);
+};
+
+/**** Public function declarations ****/
+
+#ifdef TESTING
+#endif
+
+} //namespace
+
+#endif /* DISPLAY_LED_H_ */

firmware/src/hw/potentiometer.cpp

@@ -0,0 +1,38 @@
+/**** Includes ****/
+#include "../utils/utils.h"
+#include "../utils/interpolate.h"
+#include "potentiometer.h"
+
+using namespace hw;
+
+/**** Private definitions ****/
+/**** Private constants ****/
+/**** Private variables ****/
+/**** Private function declarations ****/
+
+/**** Public function definitions ****/
+hw::Potentiometer::Potentiometer(board::AnalogIn* ain_ch, uint16_t low_deadzone, uint16_t high_deadzone)
+{
+	this->ain_ch = ain_ch;
+	this->low_deadzone = low_deadzone;
+	this->high_deadzone = high_deadzone;
+	this->percent = 0;
+}
+
+hw::Potentiometer::~Potentiometer(void)
+{
+	return;
+}
+
+uint8_t hw::Potentiometer::read(void)
+{
+	// Update input
+	this->ain_ch->read();
+	
+	// Calculate percent
+	this->percent = util::interpolate(this->ain_ch->last_read, this->low_deadzone, this->high_deadzone, 0, 100);
+	
+	return this->percent;
+}
+
+/**** Private function definitions ****/

firmware/src/hw/potentiometer.h

@@ -0,0 +1,35 @@
+#ifndef POTENTIOMETER_H_
+#define POTENTIOMETER_H_
+
+/**** Includes ****/
+#include <stdint.h>
+#include "../board/ain.h"
+
+namespace hw {
+
+/**** Public definitions ****/
+
+class Potentiometer
+{
+	protected:
+	board::AnalogIn* ain_ch;
+	
+	public:
+	Potentiometer(board::AnalogIn* ain_ch, uint16_t low_deadzone, uint16_t high_deadzone);
+	~Potentiometer(void);
+	
+	uint16_t low_deadzone;
+	uint16_t high_deadzone;
+	uint8_t percent;
+	
+	uint8_t read(void);
+};
+
+/**** Public function declarations ****/
+
+#ifdef TESTING
+#endif
+
+} //namespace
+
+#endif /* POTENTIOMETER_H_ */

firmware/src/main.cpp

@@ -0,0 +1,117 @@
+/**** Includes ****/
+#include "utils/utils.h"
+
+#include "board/mcu/mcu_hal.h"
+#include "board/ain.h"
+#include "board/din.h"
+#include "board/dout.h"
+#include "board/dio.h"
+#include "board/halfbridge.h"
+#include "board/pwm.h"
+
+#include "hw/button.h"
+#include "hw/potentiometer.h"
+#include "hw/display_led.h"
+#include "hw/cv_output.h"
+
+/**** Private definitions ****/
+static board::AnalogIn dccd_i(mcu::ADC0);
+static board::AnalogIn dccd_u(mcu::ADC1);
+static board::AnalogIn bat_u(mcu::ADC2);
+static board::AnalogIn bat_i(mcu::ADC3);
+
+static board::Hafbridge hbridge(mcu::PWM0, mcu::GPIO15, 95);
+
+static board::AnalogIn ain1(mcu::ADC5); // mode
+static board::AnalogIn ain2(mcu::ADC4); // pot
+
+static board::DigitalIn din1(mcu::GPIO0, 0, board::DIN_HIGH); //mode
+static board::DigitalIn din2(mcu::GPIO1, 0, board::DIN_HIGH); //pot
+static board::DigitalIn din3(mcu::GPIO2, 0, board::DIN_HIGH); //down
+static board::DigitalIn din4(mcu::GPIO3, 0, board::DIN_HIGH); //up
+
+static board::DigitalIn hvdin1(mcu::GPIO4, 1, board::DIN_LOW); //dimm
+static board::DigitalIn hvdin2(mcu::GPIO5, 1, board::DIN_LOW); //brakes
+static board::DigitalIn hvdin3(mcu::GPIO6, 1, board::DIN_LOW); //hbrake
+static board::DigitalIO hvdin3_pull(mcu::GPIO7, board::DIN_HIGH); //hbrake pull
+
+static board::DigitalOut odout1(mcu::GPIO9,  1);
+static board::DigitalOut odout2(mcu::GPIO10, 1);
+static board::DigitalOut odout3(mcu::GPIO11, 1);
+static board::DigitalOut odout4(mcu::GPIO12, 1);
+static board::DigitalOut odout5(mcu::GPIO13, 1);
+static board::DigitalOut odout6(mcu::GPIO14, 1);
+
+static board::PWMout od_pwm(mcu::PWM1);
+
+static hw::Button btn_mode(&din1, board::DIN_LOW, 10, hw::BUTTON_OFF);
+static hw::Button btn_up(&din4, board::DIN_LOW, 10, hw::BUTTON_OFF);
+static hw::Button btn_down(&din3, board::DIN_LOW, 10, hw::BUTTON_OFF);
+
+static hw::Button sw_dimm(&hvdin1, board::DIN_HIGH, 10, hw::BUTTON_OFF);
+static hw::Button sw_brakes(&hvdin2, board::DIN_HIGH, 10, hw::BUTTON_OFF);
+static hw::Button sw_hbrake(&hvdin3, board::DIN_LOW, 10, hw::BUTTON_OFF);
+
+static hw::Potentiometer pot(&ain2, 500, 4500);
+
+static hw::DisplayLed display(&odout1, &odout2, &odout3, &odout4, &odout5, &odout6, &od_pwm);
+
+static hw::CVoutput cvout(&hbridge);
+
+/**** Private constants ****/
+/**** Private variables ****/
+/**** Private function declarations ****/
+static void board_setup(void);
+
+/**** Public function definitions ****/
+int main(void)
+{
+	board_setup();
+
+	cvout.target = 0;
+	cvout.min_out = 500;
+	cvout.enable();
+
+	// Super loop
+	while(1)
+	{				
+		continue; // End of super loop
+	}
+	
+	// Escape the matrix
+	return 0;
+}
+
+/**** Private function definitions ***/
+static void board_setup(void)
+{
+	mcu::startupCfg_t mcu_cfg;
+	mcu_cfg.adc_clk = mcu::ADC_DIV2;
+	mcu_cfg.pwm_clk = mcu::TIM_DIV1;
+	mcu_cfg.pwm_top = 200;
+	mcu_cfg.pwm_ch1_en = 1;
+	
+	mcu::startup(&mcu_cfg);
+	
+	dccd_i.mul = 215;
+	dccd_i.div = 22;
+	dccd_i.offset = 0;
+	dccd_i.last_read = 0;
+	
+	dccd_u.mul = 20;
+	dccd_u.div = 1;
+	dccd_u.offset = 0;
+	dccd_u.last_read = 0;
+	
+	bat_u.mul = 20;
+	bat_u.div = 1;
+	bat_u.offset = 0;
+	bat_u.last_read = 12000;
+	
+	bat_i.mul = 235;
+	bat_i.div = 6;
+	bat_i.offset = 0;
+	bat_i.last_read = 0;
+	
+	od_pwm.write(100);
+}

firmware/src/uDCCD.atsln

@@ -0,0 +1,22 @@
+
+Microsoft Visual Studio Solution File, Format Version 12.00
+# Atmel Studio Solution File, Format Version 11.00
+VisualStudioVersion = 14.0.23107.0
+MinimumVisualStudioVersion = 10.0.40219.1
+Project("{E66E83B9-2572-4076-B26E-6BE79FF3018A}") = "uDCCD", "uDCCD.cppproj", "{DCE6C7E3-EE26-4D79-826B-08594B9AD897}"
+EndProject
+Global
+	GlobalSection(SolutionConfigurationPlatforms) = preSolution
+		Debug|AVR = Debug|AVR
+		Release|AVR = Release|AVR
+	EndGlobalSection
+	GlobalSection(ProjectConfigurationPlatforms) = postSolution
+		{DCE6C7E3-EE26-4D79-826B-08594B9AD897}.Debug|AVR.ActiveCfg = Debug|AVR
+		{DCE6C7E3-EE26-4D79-826B-08594B9AD897}.Debug|AVR.Build.0 = Debug|AVR
+		{DCE6C7E3-EE26-4D79-826B-08594B9AD897}.Release|AVR.ActiveCfg = Release|AVR
+		{DCE6C7E3-EE26-4D79-826B-08594B9AD897}.Release|AVR.Build.0 = Release|AVR
+	EndGlobalSection
+	GlobalSection(SolutionProperties) = preSolution
+		HideSolutionNode = FALSE
+	EndGlobalSection
+EndGlobal

firmware/src/uDCCD.componentinfo.xml

@@ -0,0 +1,86 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Store xmlns:i="http://www.w3.org/2001/XMLSchema-instance" xmlns="AtmelPackComponentManagement">
+	<ProjectComponents>
+		<ProjectComponent z:Id="i1" xmlns:z="http://schemas.microsoft.com/2003/10/Serialization/">
+			<CApiVersion></CApiVersion>
+			<CBundle></CBundle>
+			<CClass>Device</CClass>
+			<CGroup>Startup</CGroup>
+			<CSub></CSub>
+			<CVariant></CVariant>
+			<CVendor>Atmel</CVendor>
+			<CVersion>1.7.0</CVersion>
+			<DefaultRepoPath>C:/Program Files (x86)\Atmel\Studio\7.0\Packs</DefaultRepoPath>
+			<DependentComponents xmlns:d4p1="http://schemas.microsoft.com/2003/10/Serialization/Arrays" />
+			<Description></Description>
+			<Files xmlns:d4p1="http://schemas.microsoft.com/2003/10/Serialization/Arrays">
+				<d4p1:anyType i:type="FileInfo">
+					<AbsolutePath>C:/Program Files (x86)\Atmel\Studio\7.0\Packs\atmel\ATmega_DFP\1.7.374\include\</AbsolutePath>
+					<Attribute></Attribute>
+					<Category>include</Category>
+					<Condition>C</Condition>
+					<FileContentHash i:nil="true" />
+					<FileVersion></FileVersion>
+					<Name>include/</Name>
+					<SelectString></SelectString>
+					<SourcePath></SourcePath>
+				</d4p1:anyType>
+				<d4p1:anyType i:type="FileInfo">
+					<AbsolutePath>C:/Program Files (x86)\Atmel\Studio\7.0\Packs\atmel\ATmega_DFP\1.7.374\include\avr\iom328pb.h</AbsolutePath>
+					<Attribute></Attribute>
+					<Category>header</Category>
+					<Condition>C</Condition>
+					<FileContentHash>TU9y07FA4IWGxznrvGv9rQ==</FileContentHash>
+					<FileVersion></FileVersion>
+					<Name>include/avr/iom328pb.h</Name>
+					<SelectString></SelectString>
+					<SourcePath></SourcePath>
+				</d4p1:anyType>
+				<d4p1:anyType i:type="FileInfo">
+					<AbsolutePath>C:/Program Files (x86)\Atmel\Studio\7.0\Packs\atmel\ATmega_DFP\1.7.374\templates\main.c</AbsolutePath>
+					<Attribute>template</Attribute>
+					<Category>source</Category>
+					<Condition>C Exe</Condition>
+					<FileContentHash>KjvOcFWd++tbnsEMfVPd/w==</FileContentHash>
+					<FileVersion></FileVersion>
+					<Name>templates/main.c</Name>
+					<SelectString>Main file (.c)</SelectString>
+					<SourcePath></SourcePath>
+				</d4p1:anyType>
+				<d4p1:anyType i:type="FileInfo">
+					<AbsolutePath>C:/Program Files (x86)\Atmel\Studio\7.0\Packs\atmel\ATmega_DFP\1.7.374\templates\main.cpp</AbsolutePath>
+					<Attribute>template</Attribute>
+					<Category>source</Category>
+					<Condition>C Exe</Condition>
+					<FileContentHash>w5aB/d0+DbxGZ7yY0aMMjw==</FileContentHash>
+					<FileVersion></FileVersion>
+					<Name>templates/main.cpp</Name>
+					<SelectString>Main file (.cpp)</SelectString>
+					<SourcePath></SourcePath>
+				</d4p1:anyType>
+				<d4p1:anyType i:type="FileInfo">
+					<AbsolutePath>C:/Program Files (x86)\Atmel\Studio\7.0\Packs\atmel\ATmega_DFP\1.7.374\gcc\dev\atmega328pb</AbsolutePath>
+					<Attribute></Attribute>
+					<Category>libraryPrefix</Category>
+					<Condition>GCC</Condition>
+					<FileContentHash i:nil="true" />
+					<FileVersion></FileVersion>
+					<Name>gcc/dev/atmega328pb</Name>
+					<SelectString></SelectString>
+					<SourcePath></SourcePath>
+				</d4p1:anyType>
+			</Files>
+			<PackName>ATmega_DFP</PackName>
+			<PackPath>C:/Program Files (x86)/Atmel/Studio/7.0/Packs/atmel/ATmega_DFP/1.7.374/Atmel.ATmega_DFP.pdsc</PackPath>
+			<PackVersion>1.7.374</PackVersion>
+			<PresentInProject>true</PresentInProject>
+			<ReferenceConditionId>ATmega328PB</ReferenceConditionId>
+			<RteComponents xmlns:d4p1="http://schemas.microsoft.com/2003/10/Serialization/Arrays">
+				<d4p1:string></d4p1:string>
+			</RteComponents>
+			<Status>Resolved</Status>
+			<VersionMode>Fixed</VersionMode>
+			<IsComponentInAtProject>true</IsComponentInAtProject>
+		</ProjectComponent>
+	</ProjectComponents>
+</Store>

firmware/src/uDCCD.cppproj

@@ -0,0 +1,245 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Project DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003" ToolsVersion="14.0">
+  <PropertyGroup>
+    <SchemaVersion>2.0</SchemaVersion>
+    <ProjectVersion>7.0</ProjectVersion>
+    <ToolchainName>com.Atmel.AVRGCC8.CPP</ToolchainName>
+    <ProjectGuid>dce6c7e3-ee26-4d79-826b-08594b9ad897</ProjectGuid>
+    <avrdevice>ATmega328PB</avrdevice>
+    <avrdeviceseries>none</avrdeviceseries>
+    <OutputType>Executable</OutputType>
+    <Language>CPP</Language>
+    <OutputFileName>$(MSBuildProjectName)</OutputFileName>
+    <OutputFileExtension>.elf</OutputFileExtension>
+    <OutputDirectory>$(MSBuildProjectDirectory)\$(Configuration)</OutputDirectory>
+    <AssemblyName>uDCCD</AssemblyName>
+    <Name>uDCCD</Name>
+    <RootNamespace>uDCCD</RootNamespace>
+    <ToolchainFlavour>Native</ToolchainFlavour>
+    <KeepTimersRunning>true</KeepTimersRunning>
+    <OverrideVtor>false</OverrideVtor>
+    <CacheFlash>true</CacheFlash>
+    <ProgFlashFromRam>true</ProgFlashFromRam>
+    <RamSnippetAddress />
+    <UncachedRange />
+    <preserveEEPROM>true</preserveEEPROM>
+    <OverrideVtorValue />
+    <BootSegment>2</BootSegment>
+    <ResetRule>0</ResetRule>
+    <eraseonlaunchrule>0</eraseonlaunchrule>
+    <EraseKey />
+    <AsfFrameworkConfig>
+      <framework-data xmlns="">
+        <options />
+        <configurations />
+        <files />
+        <documentation help="" />
+        <offline-documentation help="" />
+        <dependencies>
+          <content-extension eid="atmel.asf" uuidref="Atmel.ASF" version="3.52.0" />
+        </dependencies>
+      </framework-data>
+    </AsfFrameworkConfig>
+  </PropertyGroup>
+  <PropertyGroup Condition=" '$(Configuration)' == 'Release' ">
+    <ToolchainSettings>
+      <AvrGccCpp>
+        <avrgcc.common.Device>-mmcu=atmega328pb -B "%24(PackRepoDir)\atmel\ATmega_DFP\1.7.374\gcc\dev\atmega328pb"</avrgcc.common.Device>
+        <avrgcc.common.outputfiles.hex>True</avrgcc.common.outputfiles.hex>
+        <avrgcc.common.outputfiles.lss>True</avrgcc.common.outputfiles.lss>
+        <avrgcc.common.outputfiles.eep>True</avrgcc.common.outputfiles.eep>
+        <avrgcc.common.outputfiles.srec>True</avrgcc.common.outputfiles.srec>
+        <avrgcc.common.outputfiles.usersignatures>False</avrgcc.common.outputfiles.usersignatures>
+        <avrgcc.compiler.general.ChangeDefaultCharTypeUnsigned>True</avrgcc.compiler.general.ChangeDefaultCharTypeUnsigned>
+        <avrgcc.compiler.general.ChangeDefaultBitFieldUnsigned>True</avrgcc.compiler.general.ChangeDefaultBitFieldUnsigned>
+        <avrgcc.compiler.symbols.DefSymbols>
+          <ListValues>
+            <Value>NDEBUG</Value>
+          </ListValues>
+        </avrgcc.compiler.symbols.DefSymbols>
+        <avrgcc.compiler.directories.IncludePaths>
+          <ListValues>
+            <Value>%24(PackRepoDir)\atmel\ATmega_DFP\1.7.374\include\</Value>
+          </ListValues>
+        </avrgcc.compiler.directories.IncludePaths>
+        <avrgcc.compiler.optimization.level>Optimize for size (-Os)</avrgcc.compiler.optimization.level>
+        <avrgcc.compiler.optimization.PackStructureMembers>True</avrgcc.compiler.optimization.PackStructureMembers>
+        <avrgcc.compiler.optimization.AllocateBytesNeededForEnum>True</avrgcc.compiler.optimization.AllocateBytesNeededForEnum>
+        <avrgcc.compiler.warnings.AllWarnings>True</avrgcc.compiler.warnings.AllWarnings>
+        <avrgcccpp.compiler.general.ChangeDefaultCharTypeUnsigned>True</avrgcccpp.compiler.general.ChangeDefaultCharTypeUnsigned>
+        <avrgcccpp.compiler.general.ChangeDefaultBitFieldUnsigned>True</avrgcccpp.compiler.general.ChangeDefaultBitFieldUnsigned>
+        <avrgcccpp.compiler.symbols.DefSymbols>
+          <ListValues>
+            <Value>NDEBUG</Value>
+          </ListValues>
+        </avrgcccpp.compiler.symbols.DefSymbols>
+        <avrgcccpp.compiler.directories.IncludePaths>
+          <ListValues>
+            <Value>%24(PackRepoDir)\atmel\ATmega_DFP\1.7.374\include\</Value>
+          </ListValues>
+        </avrgcccpp.compiler.directories.IncludePaths>
+        <avrgcccpp.compiler.optimization.level>Optimize for size (-Os)</avrgcccpp.compiler.optimization.level>
+        <avrgcccpp.compiler.optimization.PackStructureMembers>True</avrgcccpp.compiler.optimization.PackStructureMembers>
+        <avrgcccpp.compiler.optimization.AllocateBytesNeededForEnum>True</avrgcccpp.compiler.optimization.AllocateBytesNeededForEnum>
+        <avrgcccpp.compiler.warnings.AllWarnings>True</avrgcccpp.compiler.warnings.AllWarnings>
+        <avrgcccpp.linker.libraries.Libraries>
+          <ListValues>
+            <Value>libm</Value>
+          </ListValues>
+        </avrgcccpp.linker.libraries.Libraries>
+        <avrgcccpp.assembler.general.IncludePaths>
+          <ListValues>
+            <Value>%24(PackRepoDir)\atmel\ATmega_DFP\1.7.374\include\</Value>
+          </ListValues>
+        </avrgcccpp.assembler.general.IncludePaths>
+      </AvrGccCpp>
+    </ToolchainSettings>
+  </PropertyGroup>
+  <PropertyGroup Condition=" '$(Configuration)' == 'Debug' ">
+    <ToolchainSettings>
+      <AvrGccCpp>
+        <avrgcc.common.Device>-mmcu=atmega328pb -B "%24(PackRepoDir)\atmel\ATmega_DFP\1.7.374\gcc\dev\atmega328pb"</avrgcc.common.Device>
+        <avrgcc.common.outputfiles.hex>True</avrgcc.common.outputfiles.hex>
+        <avrgcc.common.outputfiles.lss>True</avrgcc.common.outputfiles.lss>
+        <avrgcc.common.outputfiles.eep>True</avrgcc.common.outputfiles.eep>
+        <avrgcc.common.outputfiles.srec>True</avrgcc.common.outputfiles.srec>
+        <avrgcc.common.outputfiles.usersignatures>False</avrgcc.common.outputfiles.usersignatures>
+        <avrgcc.compiler.general.ChangeDefaultCharTypeUnsigned>True</avrgcc.compiler.general.ChangeDefaultCharTypeUnsigned>
+        <avrgcc.compiler.general.ChangeDefaultBitFieldUnsigned>True</avrgcc.compiler.general.ChangeDefaultBitFieldUnsigned>
+        <avrgcc.compiler.symbols.DefSymbols>
+          <ListValues>
+            <Value>DEBUG</Value>
+          </ListValues>
+        </avrgcc.compiler.symbols.DefSymbols>
+        <avrgcc.compiler.directories.IncludePaths>
+          <ListValues>
+            <Value>%24(PackRepoDir)\atmel\ATmega_DFP\1.7.374\include\</Value>
+          </ListValues>
+        </avrgcc.compiler.directories.IncludePaths>
+        <avrgcc.compiler.optimization.level>Optimize debugging experience (-Og)</avrgcc.compiler.optimization.level>
+        <avrgcc.compiler.optimization.PackStructureMembers>True</avrgcc.compiler.optimization.PackStructureMembers>
+        <avrgcc.compiler.optimization.AllocateBytesNeededForEnum>True</avrgcc.compiler.optimization.AllocateBytesNeededForEnum>
+        <avrgcc.compiler.optimization.DebugLevel>Default (-g2)</avrgcc.compiler.optimization.DebugLevel>
+        <avrgcc.compiler.warnings.AllWarnings>True</avrgcc.compiler.warnings.AllWarnings>
+        <avrgcccpp.compiler.general.ChangeDefaultCharTypeUnsigned>True</avrgcccpp.compiler.general.ChangeDefaultCharTypeUnsigned>
+        <avrgcccpp.compiler.general.ChangeDefaultBitFieldUnsigned>True</avrgcccpp.compiler.general.ChangeDefaultBitFieldUnsigned>
+        <avrgcccpp.compiler.symbols.DefSymbols>
+          <ListValues>
+            <Value>DEBUG</Value>
+          </ListValues>
+        </avrgcccpp.compiler.symbols.DefSymbols>
+        <avrgcccpp.compiler.directories.IncludePaths>
+          <ListValues>
+            <Value>%24(PackRepoDir)\atmel\ATmega_DFP\1.7.374\include\</Value>
+          </ListValues>
+        </avrgcccpp.compiler.directories.IncludePaths>
+        <avrgcccpp.compiler.optimization.level>Optimize debugging experience (-Og)</avrgcccpp.compiler.optimization.level>
+        <avrgcccpp.compiler.optimization.PackStructureMembers>True</avrgcccpp.compiler.optimization.PackStructureMembers>
+        <avrgcccpp.compiler.optimization.AllocateBytesNeededForEnum>True</avrgcccpp.compiler.optimization.AllocateBytesNeededForEnum>
+        <avrgcccpp.compiler.optimization.DebugLevel>Default (-g2)</avrgcccpp.compiler.optimization.DebugLevel>
+        <avrgcccpp.compiler.warnings.AllWarnings>True</avrgcccpp.compiler.warnings.AllWarnings>
+        <avrgcccpp.linker.libraries.Libraries>
+          <ListValues>
+            <Value>libm</Value>
+          </ListValues>
+        </avrgcccpp.linker.libraries.Libraries>
+        <avrgcccpp.assembler.general.IncludePaths>
+          <ListValues>
+            <Value>%24(PackRepoDir)\atmel\ATmega_DFP\1.7.374\include\</Value>
+          </ListValues>
+        </avrgcccpp.assembler.general.IncludePaths>
+        <avrgcccpp.assembler.debugging.DebugLevel>Default (-Wa,-g)</avrgcccpp.assembler.debugging.DebugLevel>
+      </AvrGccCpp>
+    </ToolchainSettings>
+  </PropertyGroup>
+  <ItemGroup>
+    <Compile Include="board\ain.cpp">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="board\ain.h">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="board\din.cpp">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="board\din.h">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="board\dio.cpp">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="board\dio.h">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="board\halfbridge.cpp">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="board\halfbridge.h">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="board\mcu\mcu_hal.h">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="board\mcu\mcu_hal_r8.cpp">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="board\dout.cpp">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="board\dout.h">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="board\pwm.cpp">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="board\pwm.h">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="hw\button.cpp">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="hw\button.h">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="hw\cv_otput.cpp">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="hw\cv_output.h">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="hw\display_led.cpp">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="hw\display_led.h">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="hw\potentiometer.cpp">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="hw\potentiometer.h">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="main.cpp">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="utils\interpolate.cpp">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="utils\interpolate.h">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="utils\utils.cpp">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="utils\utils.h">
+      <SubType>compile</SubType>
+    </Compile>
+  </ItemGroup>
+  <ItemGroup>
+    <Folder Include="board" />
+    <Folder Include="board\mcu" />
+    <Folder Include="hw" />
+    <Folder Include="utils" />
+  </ItemGroup>
+  <Import Project="$(AVRSTUDIO_EXE_PATH)\\Vs\\Compiler.targets" />
+</Project>

firmware/src/utils/interpolate.cpp

@@ -0,0 +1,163 @@
+/**** Includes ****/
+#include "utils.h"
+#include "interpolate.h"
+
+using namespace util;
+
+/**** Private definitions ****/
+/**** Private constants ****/
+/**** Private variables ****/
+/**** Private function declarations ****/
+/**** Public function definitions ****/
+uint16_t util::interpolate_1d(uint16_t x, uint16_t* x_axis, uint16_t* y_values, uint8_t len_axis)
+{
+	// validate axis length
+	if(len_axis==0) return 0;			// Empty data set
+	if(len_axis==1) return y_values[0];	// Only one data point
+
+	uint16_t y;
+
+	uint8_t i = find_interval_end_index(x, x_axis, len_axis);
+	if(i==0)
+	{
+		//Less then start
+		y = y_values[0];
+	}
+	else if(i==len_axis)
+	{
+		//More than end
+		y = y_values[len_axis-1];
+	}
+	else
+	{
+		// Do interpolate
+		y = interpolate(x, x_axis[i-1], x_axis[i], y_values[i-1], y_values[i]);
+	}
+
+	return y;
+}
+
+uint16_t util::interpolate_2d(uint16_t x, uint16_t y, uint16_t* x_axis, uint8_t len_x_axis, uint16_t* y_axis, uint8_t len_y_axis, uint16_t* z_values)
+{
+	// validate axis length
+	if((len_x_axis==0)&&(len_y_axis==0)) return 0;				// Empty data set
+	if((len_x_axis==1)&&(len_y_axis==1)) return z_values[0];	// Only one data point
+
+	uint8_t ix = find_interval_end_index(x, x_axis, len_x_axis);
+	uint8_t iy = find_interval_end_index(y, y_axis, len_y_axis);
+
+	// Check corners - easy answers
+	if((ix==0)&&(iy==0))
+	{
+		return z_values[0];  //[0][0]  [Y][X]
+	}
+	else if((ix==len_x_axis)&&(iy==0))
+	{
+		return z_values[len_x_axis-1]; //[0][end]
+	}
+	else if((ix==0)&&(iy==len_y_axis))
+	{
+		uint16_t i = index2d_to_index1d(0, len_y_axis-1, len_x_axis);
+		return z_values[i]; //[end][0]
+	}
+	else if((ix==len_x_axis)&&(iy==len_y_axis))
+	{
+		uint16_t i = index2d_to_index1d(len_x_axis-1, len_y_axis-1, len_x_axis);
+		return z_values[i];  //[end][end]
+	};
+
+	// Check boundaries - 1D interpolation
+	if(ix==0)
+    {
+        // On ix=0 line
+        uint16_t i = 0;
+        uint16_t z0 = z_values[i];
+        i = index2d_to_index1d(0, len_y_axis-1, len_x_axis);
+        uint16_t z1 = z_values[i];
+        return interpolate(y, y_axis[0], y_axis[len_y_axis-1], z0, z1);
+    }
+    else if(ix==len_x_axis)
+    {
+        // On ix=END line
+        uint16_t i = len_x_axis-1;
+        uint16_t z0 = z_values[i];
+        i = index2d_to_index1d(len_x_axis-1, len_y_axis-1, len_x_axis);
+        uint16_t z1 = z_values[i];
+        return interpolate(y, y_axis[0], y_axis[len_y_axis-1], z0, z1);
+    }
+    else if(iy==0)
+    {
+        // On iy=0 line
+        uint16_t i = 0;
+        uint16_t z0 = z_values[i];
+        i = len_x_axis-1;
+        uint16_t z1 = z_values[i];
+        return interpolate(x, x_axis[0], x_axis[len_x_axis-1], z0, z1);
+    }
+    else if(iy==len_y_axis)
+    {
+        // On iy=END line
+        uint16_t i = index2d_to_index1d(0, len_y_axis-1, len_x_axis);
+        uint16_t z0 = z_values[i];
+        i = index2d_to_index1d(len_x_axis-1, len_y_axis-1, len_x_axis);
+        uint16_t z1 = z_values[i];
+        return interpolate(x, x_axis[0], x_axis[len_x_axis-1], z0, z1);
+    }
+
+	// Do interpolation
+	// Get axis values
+	uint16_t x0 = x_axis[ix-1];
+	uint16_t x1 = x_axis[ix];
+	uint16_t y0 = y_axis[iy-1];
+	uint16_t y1 = y_axis[iy];
+
+	// Do y0 line calculation
+	// Get z values at x0 and x1 points on y0 line
+	uint16_t i = index2d_to_index1d(ix-1, iy-1, len_x_axis);
+	uint16_t z0 = z_values[i];
+	uint16_t z1 = z_values[i+1];
+	// Interpolate z value on y0 line
+	uint16_t zy0 = interpolate(x, x0, x1, z0, z1);
+
+	// Do y1 line calculation
+	// Get z values at x0 and x1 points on y1 line
+	i = index2d_to_index1d(ix-1, iy, len_x_axis);
+	z0 = z_values[i];
+	z1 = z_values[i+1];
+	// Interpolate z value on y0 line
+	uint16_t zy1 = interpolate(x, x0, x1, z0, z1);
+
+	// Do calculation in y axis on xz line
+	return interpolate(y, y0, y1, zy0, zy1);
+}
+
+uint16_t interpolate(uint16_t x, uint16_t x0, uint16_t x1, uint16_t y0, uint16_t y1)
+{
+	int32_t dy = (int32_t)y1 - (int32_t)y0;
+	int32_t dx = (int32_t)x1 - (int32_t)x0;
+	int32_t d = (int32_t)x - (int32_t)x0;
+
+	int32_t y = dy * d;
+	y /= dx;
+	y += y0;
+
+	return util::sat_cast(y);
+}
+
+uint8_t find_interval_end_index(uint16_t val, uint16_t* axis_values, uint8_t len_axis)
+{
+	for(uint8_t i=0; i<len_axis; i++)
+	{
+		if(val < axis_values[i]) return i;
+		continue;
+	}
+
+	return len_axis;
+}
+
+uint16_t index2d_to_index1d(uint8_t ix, uint8_t iy, uint8_t len_x)
+{
+	return ((uint16_t)len_x * iy) + ix;
+}
+
+/**** Private function definitions ****/

firmware/src/utils/interpolate.h

@@ -0,0 +1,23 @@
+#ifndef UTILS_INTERPOLATE_H_
+#define UTILS_INTERPOLATE_H_
+
+/**** Includes ****/
+#include <stdint.h>
+
+namespace util {
+
+/**** Public definitions ****/
+/**** Public function declarations ****/
+uint16_t interpolate_1d(uint16_t x, uint16_t* x_axis, uint16_t* y_values, uint8_t len_axis);
+uint16_t interpolate_2d(uint16_t x, uint16_t y, uint16_t* x_axis, uint8_t len_x_axis, uint16_t* y_axis, uint8_t len_y_axis, uint16_t* z_values);
+
+uint8_t find_interval_end_index(uint16_t val, uint16_t* axis_values, uint8_t len_axis);
+uint16_t interpolate(uint16_t x, uint16_t x0, uint16_t x1, uint16_t y0, uint16_t y1);
+uint16_t index2d_to_index1d(uint8_t ix, uint8_t iy, uint8_t len_x);
+
+#ifdef TESTING
+#endif
+
+} //namespace
+
+#endif /* UTILS_INTERPOLATE_H_ */

firmware/src/utils/utils.cpp

@@ -0,0 +1,137 @@
+/**** Includes ****/
+#include "utils.h"
+
+using namespace util;
+
+/**** Private definitions ****/
+/**** Private constants ****/
+/**** Private variables ****/
+/**** Private function declarations ****/
+#ifndef TESTING
+#endif
+
+/**** Public function definitions ****/
+uint8_t util::invert(uint8_t x)
+{
+	if(x!=0) return 0;
+	else return 1;
+}
+
+uint8_t util::sat_add(uint8_t x, uint8_t y)
+{
+	uint8_t z = x + y;
+	// Check for overflow
+	if((z < x)||(z < y)) return 0xFF;
+	else return z;
+}
+
+uint16_t util::sat_add(uint16_t x, uint16_t y)
+{
+	uint16_t z = x + y;
+	// Check for overflow
+	if((z < x)||(z < y)) return 0xFF;
+	else return z;
+}
+
+uint32_t util::sat_add(uint32_t x, uint32_t y)
+
+{
+	uint32_t z = x + y;
+	// Check for overflow
+	if((z < x)||(z < y)) return 0xFF;
+	else return z;
+}
+
+uint8_t util::sat_subtract(uint8_t x, uint8_t y)
+{
+	uint8_t z = x - y;
+	// Check for underflow
+	if(z > x) return 0;
+	else return z;
+}
+
+uint16_t util::sat_subtract(uint16_t x, uint16_t y)
+{
+	uint16_t z = x - y;
+	// Check for underflow
+	if(z > x) return 0;
+	else return z;
+}
+
+uint32_t util::sat_subtract(uint32_t x, uint32_t y)
+{
+	uint32_t z = x - y;
+	// Check for underflow
+	if(z > x) return 0;
+	else return z;
+}
+
+uint16_t util::sat_cast(uint32_t x)
+{
+	if(x > 0x0000FFFF) return 0xFFFF;
+	else return (uint16_t)x;
+}
+
+uint16_t util::sat_cast(int32_t x)
+{
+	if(x < 0) return 0x0000;
+	else if(x > 0x0000FFFF) return 0xFFFF;
+	else return (uint16_t)x;
+}
+
+uint16_t util::convert_muldivoff(uint16_t raw, uint8_t mul, uint8_t div, int16_t offset)
+{
+	int32_t temp = (int32_t)raw;
+
+	temp = temp * mul;
+	if(div>1) temp /= div;
+	temp += offset;
+
+	return sat_cast(temp);
+}
+
+uint16_t util::sat_mul_kilo(uint16_t xk, uint16_t yk)
+{
+	uint32_t temp = (uint32_t)xk * (uint32_t)yk;
+	temp /= 1000;
+
+	return sat_cast(temp);
+}
+
+uint16_t util::sat_div_kilo(uint16_t top, uint16_t bot)
+{
+	//Sanity check bot
+	if(bot==0) return 0xFFFF; //aka infinity
+
+	uint32_t temp = (uint32_t)top * 1000;
+	temp /= (uint32_t)bot;
+
+	return sat_cast(temp);
+}
+
+uint16_t util::sat_ratio(uint16_t top, uint16_t bot)
+{
+	//Sanity check bot
+	if(bot==0) return 0xFFFF; //aka infinity
+
+	//Easy option
+	if(top>=bot) return 0xFFFF;
+
+	uint32_t temp = (uint32_t)top * 0x0000FFFF;
+	temp /= (uint32_t)bot;
+
+	return sat_cast(temp);
+}
+
+uint16_t util::percent_to_16b(uint8_t percent)
+{
+	uint32_t temp = (uint32_t)percent * 0x0000FFFF;
+	temp /= 100;
+
+	// Limit to 16 bits
+	uint16_t pwm = sat_cast(temp);
+
+	return pwm;
+}
+
+/**** Private function definitions ****/

firmware/src/utils/utils.h

@@ -0,0 +1,38 @@
+#ifndef UTILS_H_
+#define UTILS_H_
+
+/**** Includes ****/
+#include <stdint.h>
+
+namespace util {
+
+/**** Public definitions ****/
+/**** Public function declarations ****/
+uint8_t invert(uint8_t x);
+
+uint16_t sat_cast(uint32_t x);
+uint16_t sat_cast(int32_t x);
+
+uint16_t convert_muldivoff(uint16_t raw, uint8_t mul, uint8_t div, int16_t offset);
+uint16_t sat_mul_kilo(uint16_t xk, uint16_t yk);
+uint16_t sat_div_kilo(uint16_t top, uint16_t bot);
+uint16_t sat_ratio(uint16_t top, uint16_t bot);
+uint16_t percent_to_16b(uint8_t percent);
+
+uint8_t sat_add(uint8_t x, uint8_t y);
+uint16_t sat_add(uint16_t x, uint16_t y);
+uint32_t sat_add(uint32_t x, uint32_t y);
+
+uint8_t sat_subtract(uint8_t x, uint8_t y);
+uint16_t sat_subtract(uint16_t x, uint16_t y);
+uint32_t sat_subtract(uint32_t x, uint32_t y);
+
+uint16_t interpolate_1d(uint16_t x, uint16_t* x_axis, uint16_t* y_values, uint8_t len_axis);
+uint16_t interpolate_2d(uint16_t x, uint16_t y, uint16_t* x_axis, uint8_t len_x_axis, uint16_t* y_axis, uint8_t len_y_axis, uint16_t* z_values);
+
+#ifdef TESTING
+#endif
+
+} //namespace
+
+#endif /* UTILS_H_ */