/*
 * dbw.c
 *
 *  Created on: May 5, 2021
 *      Author: Dmitrijs
 */

#include "main.h"


volatile config_t config_ram;
volatile var_t var;
volatile config_t *config;

volatile uint32_t ac_timer = 100, ac_mode = 0;

const config_t config_flash __attribute__((section(".config"), used)) = {
PPS2_CALC_OPTION_LINEAR,
TPS2_CALC_OPTION_LINEAR,

100, 3990,

100, 2048, 500, 3990,

100, 3990,

100, 2048, 500, 3990,

PPS2TPS_OPTION_CURVE,
IDLE_OPTION_NO_IDLE_INPUT,

		{ 0, 67, 132, 198, 264, 330, 396, 462, 528, 594, 660, 726, 792, 858,
				924, 1000 }, { 0, 67, 132, 198, 264, 330, 396, 462, 528, 594,
				660, 726, 792, 858, 924, 1000 },

		// Idle pps modifier curve. when Idle input is used. it modifies tps target
		{ 0, 142, 284, 426, 568, 710, 852, 1000 },		//input pwm duty [0.1%]
		{ 0, 8, 17, 25, 34, 42, 51, 60 },

		1000,			// frequency in Hz
		10,		// ctl_teriod in [100uS]
		MOTOR_PID_OPTION_STATIC,// PID static coefs, static coef, but different for forward and backward moving
								// 3D table

		// PID COEFICIENTS for forward and reward
		700, 50, 10, 700, 50, 10,

		{ 0, 67, 132, 198, 264, 330, 396, 462, 528, 594, 660, 726, 792, 858,
				924, 1000 }, { -325, -281, -239, -196, -153, -110, -67, -24, 18,
				61, 104, 146, 189, 232, 275, 325 }, { { 700, 700, 700, 700, 700,
				700, 700, 700, 700, 700, 700, 700, 700, 700, 700, 700 }, { 700,
				700, 700, 700, 700, 700, 700, 700, 700, 700, 700, 700, 700, 700,
				700, 700 }, { 700, 700, 700, 700, 700, 700, 700, 700, 700, 700,
				700, 700, 700, 700, 700, 700 }, { 700, 700, 700, 700, 700, 700,
				700, 700, 700, 700, 700, 700, 700, 700, 700, 700 }, { 700, 700,
				700, 700, 700, 700, 700, 700, 700, 700, 700, 700, 700, 700, 700,
				700 }, { 700, 700, 700, 700, 700, 700, 700, 700, 700, 700, 700,
				700, 700, 700, 700, 700 }, { 700, 700, 700, 700, 700, 700, 700,
				700, 700, 700, 700, 700, 700, 700, 700, 700 },
				{ 700, 700, 700, 700, 700, 700, 700, 700, 700, 700, 700, 700,
						700, 700, 700, 700 }, { 700, 700, 700, 700, 700, 700,
						700, 700, 700, 700, 700, 700, 700, 700, 700, 700 }, {
						700, 700, 700, 700, 700, 700, 700, 700, 700, 700, 700,
						700, 700, 700, 700, 700 }, { 700, 700, 700, 700, 700,
						700, 700, 700, 700, 700, 700, 700, 700, 700, 700, 700 },
				{ 700, 700, 700, 700, 700, 700, 700, 700, 700, 700, 700, 700,
						700, 700, 700, 700 }, { 700, 700, 700, 700, 700, 700,
						700, 700, 700, 700, 700, 700, 700, 700, 700, 700 }, {
						700, 700, 700, 700, 700, 700, 700, 700, 700, 700, 700,
						700, 700, 700, 700, 700 }, { 700, 700, 700, 700, 700,
						700, 700, 700, 700, 700, 700, 700, 700, 700, 700, 700 },
				{ 700, 700, 700, 700, 700, 700, 700, 700, 700, 700, 700, 700,
						700, 700, 700, 700 }, },

		//PID limmits
		100, 10000, -10000,

		//DC offset curve
		{ 0, 67, 132, 198, 264, 330, 396, 462, 528, 594, 660, 726, 792, 858,
				924, 1000 },			// tps bins [0.1%]
		{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },				// DC

		//Battery voltage correction curve
		{ 0, 585, 1170, 1755, 2340, 2925, 3510, 4095 },	// battery voltage ADC bins []
		{ 0, 0, 0, 0, 0, 0, 0, 0 },				// motor pwm correction [0.1%]

		30, 0x100, 0x5e8, 0x00,

		//limmits
		30, 30, 15, 30, 30, 15, 100, 500, 10, 5000,

		5, 7, 		// my MS CAN ID
		1, 0, 100, 0x300 };

volatile unsigned short ctl_period;
volatile pwm_t ttl1_pwm;
volatile float pps1_gain, pps1_offset, pps2_gain, pps2_offset, tps1_gain,
		tps1_offset, tps2_gain, tps2_offset;
volatile uint16_t dbw_fast_process_timer, dbw_slow_process_timer;
volatile int32_t vbat_corr, tps_slow_t, p_comp, i_comp, d_comp, tps_error_t,
		can_target, spring_preload, idle_adder;

void Apply_Sensor_Calibration(void) {
	pps1_gain = (1000.0F / (float) (config->pps1_max - config->pps1_min));
	pps1_offset = (config->pps1_min * 1000.0F
			/ (config->pps1_min - config->pps1_max));

	pps2_gain = (1000.0F / (float) (config->pps2_max - config->pps2_min));
	pps2_offset = (config->pps2_min * 1000.0F
			/ (config->pps2_min - config->pps2_max));

	tps1_gain = (1000.0F / (float) (config->tps1_max - config->tps1_min));
	tps1_offset = (short) (config->tps1_min * 1000.0F
			/ (config->tps1_min - config->tps1_max));

	tps2_gain = (1000.0F / (float) (config->tps2_max - config->tps2_min));
	tps2_offset = (config->tps2_min * 1000.0F
			/ (config->tps2_min - config->tps2_max));

}

void DBW_Init(void) {
	dbw_fast_process_timer = 1;
	dbw_slow_process_timer = config->motor_ctl_period;
	vbat_corr = 0;
	tps_slow_t = var.tps;
	p_comp = 0;
	i_comp = 0;
	d_comp = 0;
	tps_error_t = var.tps_error;
	can_target = 0;
	spring_preload = 0;
	idle_adder = 0;
}

int DBW_Process(void) {

	int32_t pwm_output = 0;
	int32_t pct = 0;
	if (dbw_fast_process_timer == 0) {
		//process DBW every 1 mS
		dbw_fast_process_timer = 1;

		// Read all sensors and calculate error
		// calculate PPS sensor reading
		var.pps1_adc = LPF(700, Adc_Read(0), var.pps1_adc);
		var.pps1 = var.pps1_adc * pps1_gain + pps1_offset;

		var.pps2_adc = LPF(700, Adc_Read(3), var.pps2_adc); //orig 1
		var.pps2 = var.pps2_adc * pps2_gain + pps2_offset;

		var.pps_delta = var.pps1 - var.pps2;
		var.pps = (var.pps1 + var.pps2) >> 1;

		//Calculate TPS sensor reading
		var.tps1_adc = LPF(700, Adc_Read(2), var.tps1_adc);
		var.tps1 = var.tps1_adc * tps1_gain + tps1_offset;

		var.tps2_adc = LPF(700, Adc_Read(1), var.tps2_adc); //orig 3
		var.tps2 = var.tps2_adc * tps2_gain + tps2_offset;

		// calculate tps and pps delta and values
		var.tps_delta = var.tps1 - var.tps2;
		var.tps = (var.tps1 + var.tps2) >> 1;

		var.pps_delta = var.pps1 - var.pps2;
		var.pps = (var.pps1 + var.pps2) >> 1;

		// set tps target
		if (config->pps2tps_option == PPS2TPS_OPTION_CURVE) {
			var.tps_target = intrp_1d_ss_table(var.pps, 16,
					(signed short*) config->pps_bins, 1,
					(signed short*) config->tps_bins);
		}
		// if MS3 DBW protocol is used
		else if (config->pps2tps_option == PPS2TPS_OPTION_MS3_CAN) {
			// if no RX errors
			//if(var.status0 & DBW_STATUS0_CAN_MSDBW_F)
			if (config->pps2tps_option == PPS2TPS_OPTION_MS3_CAN) {
				// use MS3 TPS TARGET
				var.tps_target = can_target;
			} else {
				// on CAN error fall back to internal curve
				var.tps_target = intrp_1d_ss_table(var.pps, 16,
						(signed short*) config->pps_bins, 1,
						(signed short*) config->tps_bins);
			}
		}

		else {
			// All other TPS target options are considered as fault
			Error_Handler();
		}

		// add idle TPS target adder
		var.tps_target += idle_adder;
		//limit TPS target
		if (var.tps_target > 1000)
			var.tps_target = 1000;
		if (var.tps_target < 0)
			var.tps_target = 0;

		//calculate regulation error
		var.tps_error = var.tps_target - var.tps;

		// Proportional regulator
		p_comp = (((int32_t) var.tps_error) * (int32_t) config->motor_fw_p)
				/ 10;

		// calculate spring preload compensation using interpolation
		spring_preload = intrp_1d_ss_table(var.tps_target, 16,
				(signed short*) config->tps_dc_tps_bins, 1,
				(signed short*) config->tps_dc_offset_bins);

		if (dbw_slow_process_timer > 0) {
			dbw_slow_process_timer--;
			if (dbw_slow_process_timer == 0) {
				dbw_slow_process_timer = config->motor_ctl_period;
				//calculate slow changing thinks

				//read Battery voltage and apply corrections
				var.vbat_adc = LPF(700, Adc_Read(5), var.vbat_adc);
				pct = intrp_1d_ss_table(var.vbat_adc, 8,
						(signed short*) config->vbat_bins, 1,
						(signed short*) config->motor_pwm_corr_bins);

				//read motor current
				var.motor_current_adc = LPF(700, Adc_Read(4),
						var.motor_current_adc);

				// calculate I component

				i_comp += ((int32_t) var.tps_error
						* (int32_t) config->motor_fw_i) / 100;
				if (i_comp > config->i_limmit)
					i_comp = config->i_limmit;
				if (i_comp < (-config->i_limmit))
					i_comp = (-config->i_limmit);

				// calculate D component
				d_comp = (((int32_t) var.tps_error - tps_error_t)
						* (int32_t) config->motor_fw_d) / 10;
				tps_error_t = var.tps_error;

				// calculate single shot
				//single_shot = var.tps_error *

				// Calculate Idle adder
				if (config->idle_input_option == IDLE_OPTION_NO_IDLE_INPUT)
					idle_adder = 0;
				else if (config->idle_input_option == IDLE_OPTION_MS_CAN) {
					idle_adder = intrp_1d_ss_table(var.idle_dc, 8,
							(signed short*) config->idle_input_bins, 1,
							(signed short*) config->idle_tps_adder_bins);

				} else if (config->idle_input_option == IDLE_OPTION_PWM_INPUT1) {

					idle_adder = intrp_1d_ss_table(var.idle_dc, 8,
							(signed short*) config->idle_input_bins, 1,
							(signed short*) config->idle_tps_adder_bins)
							- (var.pps / 5);
					if (idle_adder < 0) {
						idle_adder = 0;
					}

				}

			}
		}

		pwm_output = p_comp + i_comp + d_comp + spring_preload;

		vbat_corr = (int16_t) (((int32_t) var.motor_pwm) * vbat_corr / 1000);

		pwm_output += pct;

		//limmit pwm
		if (pwm_output > config->motor_dc_max)
			pwm_output = config->motor_dc_max;
		if (pwm_output < config->motor_dc_min)
			pwm_output = config->motor_dc_min;

		//apply pwm
		DBW_Pwm_Set_Duty((signed short) (pwm_output), (pwm_t*) &ttl1_pwm);

	}

	return 0;
}

void DBW_Pwm_Init(void) {

// Setup PWM ports
	ttl1_pwm.pos_port = GPIOB;
	ttl1_pwm.pos_pin = 10;

	ttl1_pwm.neg_port = GPIOB;
	ttl1_pwm.neg_pin = 11;

// setup pwm status default -> no drive
	ttl1_pwm.status = PWM_STATUS_DEFAULT;

// calculate how many tick are in given freq period
	ttl1_pwm.period_ticks = (unsigned int) 1000000UL / config->motor_pwm_fq;

// set initian pwm from current settings
	ttl1_pwm.pwm = var.motor_pwm;
	ttl1_pwm.pwm_t = var.motor_pwm;
	ttl1_pwm.single_pulse = 0;

	//tmp = (float)ttl1_pwm.period_ticks * (float)ttl1_pwm.pwm/10000.0F;
	//ttl1_pwm.duty_ticks = (unsigned int) tmp;

	DBW_Pwm_Set_Duty(var.motor_pwm, (pwm_t*) &ttl1_pwm);

	if (var.motor_pwm < 0)
		ttl1_pwm.state = PWM_STATE_NEGATIVE_INACTIVE;
	else if (var.motor_pwm > 0)
		ttl1_pwm.state = PWM_STATE_POSITIVE_INACTIVE;
	else
		ttl1_pwm.state = PWM_STATE_POSITIVE_INACTIVE;

	ttl1_pwm.status = PWM_STATUS_IDLE;

	DBW_Stop();

	ttl1_pwm.pos_port->BSRR = (1 << (16 + ttl1_pwm.pos_pin));
	ttl1_pwm.neg_port->BSRR = (1 << (16 + ttl1_pwm.neg_pin));

// configure TIM2 to 1uS / tick timer
// setup CCR interrupt happen after sturtup delay is over

	RCC->APB1ENR |= RCC_APB1ENR_TIM2EN;
	TIM2->CNT = 0xFFFFFFFE;
	TIM2->PSC = 48; //2400;  //  (1uS precision)

	TIM2->ARR = 0xFFFFFFFF;

	TIM2->CR1 = (TIM_CR1_URS | TIM_CR1_CEN);
	TIM2->CCER = 0x0000;
	TIM2->CCMR1 = 0x0000;

	TIM2->CCR1 = (unsigned int) 20000; // startup delay
	TIM2->CCR2 = 0;
	TIM2->SR &= ~TIM_SR_CC1IF;
	TIM2->DIER |= TIM_DIER_CC1IE;
	NVIC_EnableIRQ(TIM2_IRQn);

}

void TIM2_IRQHandler(void) {
	if (TIM2->SR & TIM_SR_CC1IF) {
		// capture compare intrrupt
		TIM2->SR &= ~TIM_SR_CC1IF;
		if (ttl1_pwm.status == PWM_STATUS_DEFAULT) {
			// here ends startup delay
			// enable drive here
			ttl1_pwm.status = PWM_STATUS_STARTED;
		}

		//Set duty
		if (ttl1_pwm.pwm > 0) {
			// pwm > 0
			if ((ttl1_pwm.pwm > 0) && (ttl1_pwm.pwm_t < 0)) {
				// if changeing PWM polarity insert dead time
				ttl1_pwm.state = PWM_STATE_POSITIVE_INACTIVE;
				ttl1_pwm.pos_port->BSRR = (1 << (16 + ttl1_pwm.pos_pin));
				ttl1_pwm.neg_port->BSRR = (1 << (16 + ttl1_pwm.neg_pin));
				TIM2->CCR1 = TIM2->CNT + config->pwm_deadtime;
			} else if (ttl1_pwm.state == PWM_STATE_POSITIVE_INACTIVE) {
				ttl1_pwm.state = PWM_STATE_POSITIVE_ACTIVE;
				ttl1_pwm.pos_port->BSRR = (1 << ttl1_pwm.pos_pin);
				ttl1_pwm.neg_port->BSRR = (1 << (16 + ttl1_pwm.neg_pin));

				TIM2->CCR1 = TIM2->CNT + ttl1_pwm.duty_ticks;
			} else {
				ttl1_pwm.state = PWM_STATE_POSITIVE_INACTIVE;
				ttl1_pwm.pos_port->BSRR = (1 << (16 + ttl1_pwm.pos_pin));
				ttl1_pwm.neg_port->BSRR = (1 << (16 + ttl1_pwm.neg_pin));
				TIM2->CCR1 = TIM2->CNT
						+ (ttl1_pwm.period_ticks - ttl1_pwm.duty_ticks);

			}
		} else if (ttl1_pwm.pwm < 0) {
			//pwm < 0
			//if((ttl1_pwm.state == PWM_STATE_POSITIVE_INACTIVE) && (ttl1_pwm.state == PWM_STATE_POSITIVE_ACTIVE))
			if ((ttl1_pwm.pwm < 0) && (ttl1_pwm.pwm_t > 0)) {
				// if changeing PWM polarity insert dead time
				ttl1_pwm.state = PWM_STATE_NEGATIVE_INACTIVE;
				ttl1_pwm.pos_port->BSRR = (1 << (16 + ttl1_pwm.pos_pin));
				ttl1_pwm.neg_port->BSRR = (1 << (16 + ttl1_pwm.neg_pin));
				TIM2->CCR1 = TIM2->CNT + config->pwm_deadtime;
			} else if (ttl1_pwm.state == PWM_STATE_NEGATIVE_INACTIVE) {
				ttl1_pwm.state = PWM_STATE_NEGATIVE_ACTIVE;
				ttl1_pwm.pos_port->BSRR = (1 << (16 + ttl1_pwm.pos_pin));
				ttl1_pwm.neg_port->BSRR = (1 << ttl1_pwm.neg_pin);
				TIM2->CCR1 = TIM2->CNT + ttl1_pwm.duty_ticks;
			} else {
				ttl1_pwm.state = PWM_STATE_NEGATIVE_INACTIVE;
				ttl1_pwm.pos_port->BSRR = (1 << (16 + ttl1_pwm.pos_pin));
				ttl1_pwm.neg_port->BSRR = (1 << (16 + ttl1_pwm.neg_pin));
				TIM2->CCR1 = TIM2->CNT
						+ (ttl1_pwm.period_ticks - ttl1_pwm.duty_ticks);
			}
		} else {
			// if pwm == 0
			ttl1_pwm.pos_port->BSRR = (1 << (16 + ttl1_pwm.pos_pin));
			ttl1_pwm.neg_port->BSRR = (1 << (16 + ttl1_pwm.neg_pin));
			TIM2->CCR1 = TIM2->CNT + ttl1_pwm.period_ticks;
			if (ttl1_pwm.state == PWM_STATE_POSITIVE_ACTIVE
					|| ttl1_pwm.state == PWM_STATE_POSITIVE_INACTIVE) {
				ttl1_pwm.state = PWM_STATE_POSITIVE_INACTIVE;
			} else {
				ttl1_pwm.state = PWM_STATE_NEGATIVE_INACTIVE;
			}

		}
		// store new pwm value as old pwm value for the nex iteration
		ttl1_pwm.pwm_t = ttl1_pwm.pwm;
	}
	if (TIM2->SR & TIM_SR_UIF) {
		//TIM2 overflow interrupt jaust clear update interrupt flag
		TIM2->SR &= ~TIM_SR_UIF;

	}

}
void DBW_Pwm_Set_Duty(signed int duty, pwm_t *ttl) {
	float tmp;
	unsigned int period_ticks;

	//check limmits
	if (duty > 10000)
		duty = 10000;
	else if (duty < -10000)
		duty = -10000;

	//calculate period - needed to change frequency on-the-fly
	period_ticks = (unsigned int) 1000000UL / config->motor_pwm_fq;
	if (duty > 0)
		tmp = (float) period_ticks * (float) duty / 10000.0F;
	else if (duty < 0)
		tmp = (float) period_ticks * (0 - ((float) duty)) / 10000.0F;
	else
		tmp = 20;

	// check minimum duty period;

	// do not allow duty cylce <20uS
	if ((period_ticks - (unsigned int) tmp) < 20) {
		tmp = period_ticks - 20;
		if (duty > 0)
			duty = (signed int) tmp * 10000.0F / ttl->period_ticks;
		else if (duty < 0)
			duty = 0 - (signed int) tmp * 10000.0F / ttl->period_ticks;
	} else if (tmp < 20) {
		tmp = 20;
		duty = 0;
	}

	//update variables
	var.motor_pwm = duty;

	//apply new settings
	__disable_irq();
	ttl->duty_ticks = (unsigned int) tmp;
	ttl->period_ticks = period_ticks;
	ttl->pwm = duty;
	__enable_irq();

}

unsigned int intrp_1d_uitable(unsigned int x, unsigned char nx,
		unsigned int *x_table, unsigned int *z_table) {
	int ix;
	long interp, interp3;
	// bound input arguments

	if (x > x_table[nx - 1])
		return (z_table[nx - 1]);
	else if (x < x_table[0])
		return (z_table[0]);
	// Find bounding indiex in table
	for (ix = nx - 2; ix > -1; ix--) {  // Start w highest index
		//  because will generally have least time for calculations at hi y
		if (x > x_table[ix]) {
			break;
		}
	}
	if (ix < 0)
		ix = 0;
	// do 1D interpolate
	interp = (long) (x_table[ix + 1] - x_table[ix]);
	if (interp != 0) {
		interp3 = (long) (x - x_table[ix]);
		interp3 = (100 * interp3);
		interp = interp3 / interp;
	}
	return ((unsigned int) (z_table[ix]
			+ interp * (int) (z_table[ix + 1] - z_table[ix]) / 100));
}

signed short intrp_1d_ss_table(signed short x, unsigned char n,
		signed short *x_table, char sgn, signed short *z_table) {
	int ix;
	int interp, interp3;
	// bound input arguments
	if (x > x_table[n - 1])
		return z_table[n - 1];

	if (x < x_table[0])
		return z_table[0];

	for (ix = n - 2; ix > -1; ix--) {
		if (x > x_table[ix]) {
			break;
		}
	}
	if (ix < 0)
		ix = 0;

	interp = x_table[ix + 1] - x_table[ix];
	if (interp != 0) {
		interp3 = (x - x_table[ix]);
		interp3 = (100 * interp3);
		interp = interp3 / interp;
	}

	return ((short) ((int) z_table[ix]
			+ interp * ((int) z_table[ix + 1] - (int) z_table[ix]) / 100));
}

int intrp_1dstable(signed short x, unsigned char n, signed short *x_table,
		char sgn, signed short *z_table) {
	int ix;
	long interp, interp3;
	// bound input arguments
	if (x > x_table[n - 1]) {
		if (!sgn)
			return ((int) z_table[n - 1]);
		else
			return ((signed int) ((signed short) z_table[n - 1]));
	}
	if (x < x_table[0]) {
		if (!sgn)
			return ((signed int) z_table[0]);
		else
			return ((signed int) ((signed short) z_table[0]));
	}
	for (ix = n - 2; ix > -1; ix--) {
		if (x > x_table[ix]) {
			break;
		}
	}
	if (ix < 0)
		ix = 0;

	interp = x_table[ix + 1] - x_table[ix];
	if (interp != 0) {
		interp3 = (x - x_table[ix]);
		interp3 = (100 * interp3);
		interp = interp3 / interp;
	}
	if (!sgn)
		return ((signed int) (z_table[ix]
				+ interp * (z_table[ix + 1] - z_table[ix]) / 100));
	else
		return ((int) ((signed short) z_table[ix]
				+ interp
						* ((signed short) z_table[ix + 1]
								- (signed short) z_table[ix]) / 100));
}

void DBW_Start(void) {
	GPIOB->ODR &= ~D1_Pin;
	GPIOB->ODR |= D2_Pin;
}
void DBW_Stop(void) {
	GPIOB->ODR |= D1_Pin;
	GPIOB->ODR &= ~D2_Pin;

}

void DBW_TPS_AutoCal(void) {
// if time to process autocal
	if ((ac_timer == 0) && (ac_mode != 0)) {
		if (ac_mode == 1) {
			var.status0 |= DBW_STATUS0_PPSTPS_CAL_F;
			DBW_Pwm_Set_Duty((signed short) (-6000), (pwm_t*) &ttl1_pwm);
			DBW_Start();
			ac_timer = 1500;
			ac_mode = 2;
		} else if (ac_mode == 2) {
			uint32_t tmp = 0;
			for (int i = 0; i < 16; i++)
				tmp += Adc_Read(2);
			config->tps1_min = tmp >> 4;
			tmp = 0;
			for (int i = 0; i < 16; i++)
				tmp += Adc_Read(3);
			config->tps2_min = tmp >> 4;
			DBW_Stop();
			ac_timer = 2000;
			ac_mode = 3;
		} else if (ac_mode == 3) {
			DBW_Pwm_Set_Duty((signed short) (9000), (pwm_t*) &ttl1_pwm);
			DBW_Start();
			ac_timer = 1500;
			ac_mode = 4;
		} else if (ac_mode == 4) {
			uint32_t tmp = 0;
			for (int i = 0; i < 16; i++)
				tmp += Adc_Read(2);
			config->tps1_max = tmp >> 4;
			tmp = 0;
			for (int i = 0; i < 16; i++)
				tmp += Adc_Read(3);
			config->tps2_max = tmp >> 4;
			DBW_Stop();
			DBW_Pwm_Set_Duty((signed short) (0), (pwm_t*) &ttl1_pwm);
			Apply_Sensor_Calibration();
			Write_Config();
			ac_timer = 10000;
			ac_mode = 5;
			Comms_Reset(&RX);
			Comms_Reset(&TX);
			USART1->ICR &= ~USART_CR1_RXNEIE;
			NVIC_DisableIRQ(USART1_IRQn);

		} else if (ac_mode == 5) {
			ac_mode = 0;
			var.status0 &= ~DBW_STATUS0_PPSTPS_CAL_F;
			USART1->ICR |= USART_CR1_RXNEIE;
			NVIC_EnableIRQ(USART1_IRQn);

		}
	}

}

void DBW_Read_sensors(void) {
	var.pps1_adc = LPF(700, Adc_Read(0), var.pps1_adc);
	var.pps1 = var.pps1_adc * pps1_gain + pps1_offset;

	var.pps2_adc = LPF(700, Adc_Read(3), var.pps2_adc); // 1 is orig
	var.pps2 = var.pps2_adc * pps2_gain + pps2_offset;

	var.pps_delta = var.pps1 - var.pps2;
	var.pps = (var.pps1 + var.pps2) >> 1;

	//Calculate TPS sensor reading
	var.tps1_adc = LPF(700, Adc_Read(2), var.tps1_adc);
	var.tps1 = var.tps1_adc * tps1_gain + tps1_offset;

	var.tps2_adc = LPF(700, Adc_Read(1), var.tps2_adc); //3 is orig
	var.tps2 = var.tps2_adc * tps2_gain + tps2_offset;

	// calculate tps and pps delta and values
	var.tps_delta = var.tps1 - var.tps2;
	var.tps = (var.tps1 + var.tps2) >> 1;

	var.pps_delta = var.pps1 - var.pps2;
	var.pps = (var.pps1 + var.pps2) >> 1;
}