v0stap/DBW
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
78e6a1b8aa49f0e05ccd7f06a223d53547294c3d
DBW/DBW_V2/Core/Src/dbw.c
v0stap 78e6a1b8aa base
2026-04-05 11:31:20 +02:00

665 lines
20 KiB
C
Raw Blame History

/*
* 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;
}
Reference in New Issue View Git Blame Copy Permalink