Firmware/hpsg5-controller_v2-stm32g4
1
1
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
606090d08cdff5cf660d7f78e07e481325faad6a
hpsg5-controller_v2-stm32g4/Core/Src/toothed_wheel.c
LucianoDev 64adb64c81 Initial port
2026-03-30 18:36:35 +02:00

809 lines
22 KiB
C
Raw Blame History

/*
* toothedwheel.c
*
* Created on: Aug 4, 2025
* Author: herli
*/
#include "toothed_wheel.h"
#include "timeouts.h"
#include "injection.h"
#include "FBKW.h"
#include "can_port.h"
#include "timing_manager.h"
#include "tein_detection.h"
volatile uint8_t currentTooth = 0;
volatile uint32_t IC_RPM_Val1 = 0;
volatile uint32_t IC_RPM_Val2 = 0;
volatile uint32_t IC_MT = 0;
volatile uint32_t IC_EOI = 0;
volatile uint32_t IC_TEIN = 0;
volatile uint32_t IC_NOM_BIP = 0;
volatile uint32_t prev_Difference = 0xFFFFFFFF;
volatile uint32_t Tooth_Difference = 0;
volatile uint8_t TW_current_max_teeth = 0;
volatile uint8_t TW_RPM_SENSOR_STATE = 0;
volatile float RPM;
volatile float MT_frequency = 0;
volatile float MT_RPM = 0;
//volatile float next_MT_RPM = 0;
volatile float last_MT_RPM = 0;
float actual_phi1;
volatile float TEETH_frequency = 0;
volatile float TEETH_RPM = 0;
volatile float last_TEETHRPM = 0;
volatile uint8_t SYNCOUT_clear = 0;
volatile uint32_t RPM_Difference;
volatile uint8_t startedEngine = 0;
volatile uint8_t count_CKP = 0;
extern float MEPI;
uint8_t real_boi_pending = 0;
uint8_t real_eoi_pending = 0;
uint8_t phi1_eval_pending = 0;
uint8_t hasCapturedTeeth = 0;
uint8_t real_eoi_updated = 0;
uint8_t real_bip_updated = 0;
uint8_t eoi_eval_pending = 0;
void TW_RESET_SENSOR(){
TW_RPM_SENSOR_STATE = 0;
TW_current_max_teeth = 0;
prev_Difference = 0xFFFFFFFF;
hasCapturedTeeth = 0;
}
void TW_EVAL_MAX_TEETH(){
TW_RPM_SENSOR_STATE = (TW_current_max_teeth == TW_PERCYL_TEETH) ? 1 : 0 ;
}
uint32_t sensorfail;
volatile uint8_t isMT = 0;
uint8_t SYNCOUT_TEETH = 0;
volatile float accel_rpm = 0;
// en un timeout tendria que resetear el prevdiff, y hascapturedteet.
void TW_TEETH_CAPTURE(){
if(hasCapturedTeeth){
Tooth_Difference = IC_RPM_Val2 >= IC_RPM_Val1 ? IC_RPM_Val2-IC_RPM_Val1 : IC_RPM_Val2-(IC_RPM_Val1-0xffffffff);
}
if(Tooth_Difference > prev_Difference * TW_MT_THRESHOLD && TEETH_RPM > 2 && !isInjecting){
//esto es para que la realentizacion no nos afecte y podamos bajar el treshold
isMT = 1;
//si tiene TEETH RPM y no está inyectando.. es MT 100%
//si no hay algun diente capturado antes: esta claro que no es missing tooth
}else{
//el original, ademas checkea si esta en el ultimo diente y sabe que viene el missing tooth.
//Sigue leyendo rpm pero da error de sensor y no inyecta... habra que ver si es necesario implementarlo.
isMT = 0;
}
if(isMT){
RPM_Difference = IC_RPM_Val2 >= IC_MT ? IC_RPM_Val2-IC_MT : IC_RPM_Val2-(IC_MT-0xffffffff);
IC_MT = IC_RPM_Val2;
TW_current_max_teeth = currentTooth;
TW_RPM_SENSOR_STATE = (TW_current_max_teeth == TW_PERCYL_TEETH) ? 1 : 0 ;
currentTooth = 0;
INJ_MT_TASK();
if(!TW_current_max_teeth){
/*correction_beta = 0.0;
correction_eoi = 0.0;*/
//quitao para q no de error
//Timeout_ResetByIndex(4, TIM16->CNT); // Reset captor defect timeout
}
if(!TW_RPM_SENSOR_STATE){
ClearEdgeBuffer();
//TODO ENABLE CLEAREDGEBUFFER
//TW_RESET_SENSOR();
//TW_Reset_Pending_Tasks();
//IC_MT = 0;
sensorfail++;
hasCapturedTeeth = 0;
TEETH_RPM = 0;
return;
}
FBKW_RESET_FEEDBACK();
last_MT_RPM = MT_RPM;
MT_frequency = refClock/RPM_Difference;
MT_RPM = fclamp(60.0 * MT_frequency / CYLINDERS, MIN_RPM, 4000);
RPM=MT_RPM;
TM_UPDATE_ACCELERATION(last_MT_RPM, MT_RPM, RPM_Difference);//next_MT_RPM =
MakeEdgeBufferOld();
UpdateEdgeBuffer(IC_RPM_Val2, MT_RPM, 0, isInjecting);
TW_PREPARE_BOI_ACCELCORRECTION();
if(startup && RPM > 200){
can_port_send_msg_def(&MSG_ID_EMPF3); // TX from template (+ symbols if any)
Timeout_ResetByIndex(18, TIM16->CNT); // re-arm for another 40ms
}
INJ_PREPARE_ONCE();
//TW_CALCULATE_INSTANT_EOI_CORRECTION();
real_bip_updated = 0;
real_eoi_updated = 0;
if(currentTooth == SYNCOUT_TEETH){
if(SYNC_Pulse_IsSyncoutMode()){
SYNCOUT_clear = 1;
SYNC_Pulse_GPIO_set(1);
}
}
}else{ //normal teeth
currentTooth++;
if(!hasInjected && TW_RPM_SENSOR_STATE){ //TODO
EvaluateInjection();
/*if(currentTooth > 13){
SYNCOUT_clear = 0;
}*/
}
if(currentTooth == SYNCOUT_TEETH){
if(SYNC_Pulse_IsSyncoutMode()){
SYNCOUT_clear = 1;
SYNC_Pulse_GPIO_set(1);
}
}
if(currentTooth == SYNCOUT_TEETH + 1 && SYNCOUT_clear && SYNC_Pulse_IsSyncoutMode()){
SYNC_Pulse_GPIO_set(0);
SYNCOUT_clear = 0;
}
if(hasCapturedTeeth){
TEETH_frequency = refClock/Tooth_Difference;
TEETH_RPM = fclamp(60.0 * TEETH_frequency / TW_THEETHS, 1, 4000);
}
//UpdateEdgeBuffer(IC_RPM_Val2, TEETH_RPM, currentTooth);
if(TW_RPM_SENSOR_STATE){
if(!hasInjected){
if(startedEngine && ME > 0.031){ //hay que ver, igual hace que resetee el BOI
//TW_CALCULATE_INSTANT_BOI_CORRECTION(); //deactivated for now
}
//EvaluateInjection();
INJ_EVAL_EOI_COMPENSATION();
}
INJ_EVAL_END();
UpdateEdgeBuffer(IC_RPM_Val2, TEETH_RPM, currentTooth, isInjecting);
if(!hasInjected){
INJ_PREPARE_BIP(currentTooth);
if(TW_RPM_SENSOR_STATE){ //TODO
EvaluateInjection();
}
TW_CALCULATE_INSTANT_BOI_CORRECTION(); //deactivated for now
}
}
if(currentTooth == TW_PERCYL_TEETH){
if(RPM > 200) {
can_port_send_msg_def(&MSG_ID_EMPF1);
}
if (s_empf2_pending) {
can_port_send_msg_def(&MSG_ID_EMPF2); // packs from EMPF2_WORDS[]
s_empf2_pending = 0;
Timeout_StopByIndex(17); // stop the 60ms gate
}
if(HAL_GPIO_ReadPin(SHUT_OFF_GPIO_Port, SHUT_OFF_Pin) != GPIO_PIN_RESET){ //si esta en alto
}else{
Timeout_ResetByIndex(11, TIM16->CNT); // Reset turnoff timeout
}
}
if(currentTooth == TW_PERCYL_TEETH - 1){
INJ_END();
}
hasCapturedTeeth = 1;
if(hasInjectionEndedFlag == 1){
hasInjectionEndedFlag++;
}else if(hasInjectionEndedFlag > 1){
hasInjectionEnded = 1;
real_boi_pending = 1;
real_eoi_pending = 1;
}
if(hasTeinDetEndedFlag == 1){
hasTeinDetEndedFlag = 0;
hasTeinDetEnded = 1;
}
/*}else if(hasInjectionEndedFlag > 1){
hasTeinDetEnded = 1;
}*/
}
prev_Difference = Tooth_Difference;
IC_RPM_Val1 = IC_RPM_Val2;
if(TEETH_RPM > 15){
Timeout_ResetByIndex(3, TIM16->CNT); // Reset RPM timeout
}
if(RPM > TW_STARTED_RPM){
//startedEngine = 1;
Timeout_StartIfStopped(19, TIM16->CNT);
}
}
uint8_t ckp_process_pending = 0;
uint8_t teethCKP;
void TW_CKP_CAPTURE(){
if(TW_RPM_SENSOR_STATE){
count_CKP++;
ckp_process_pending=1;
//meter timeout cuando !count_ckp (significa que solo recibe un pulso);
CKP_PULSE_AVAILABLE = 1;
Timeout_ResetByIndex(13, TIM16->CNT); // Reset CKP timeout
}
}
float fb_1;
float fb_2;
uint8_t fb_lock;
float new_fb = 0.0;
float lpf_fb = 1.0;//0.4
uint8_t feedbackMT = 0;
uint8_t valid = 0;
float ckp_rpm = 0.0;
float fbkw_offset= 0;
void TW_CALC_FBKW_FEEDBACK(){
if(!TW_RPM_SENSOR_STATE){
return;
}
int index = -1;
uint32_t IC_CKP_Real = IC_CKP2 - FBKW_FEEDBACK_IC_DT * 16;
for(uint8_t i = 1; i < TW_PERCYL_TEETH; i++){
if(edgeBuf[i].ic < IC_CKP_Real && IC_CKP_Real < edgeBuf[i+1].ic){
index = i;
break;
}
}
if(index < 0){
if(edgeBuf[TW_PERCYL_TEETH].ic < IC_CKP_Real && IC_CKP_Real < edgeBuf[0].ic){//probar si lo que le pasa es que es el ultimo
index = TW_PERCYL_TEETH;
}else{
return;
}
}
uint8_t index0 = FBKW_FEEDBACK_ZERO / TW_TOOTH_ALPHA;
teethCKP = index;
uint8_t nteeths = TW_PERCYL_TEETH - index0 + 1;
uint32_t diffCKP_e = IC_CKP_Real >= edgeBuf[index0-1].ic ? IC_CKP_Real - edgeBuf[index0-1].ic : IC_CKP_Real - (-edgeBuf[index0-1].ic - 0xffffffff);
/*float b_a = 1.0*FBKW_FEEDBACK_ZERO / TW_TOOTH_ALPHA - index0; //remaining angle
float rpm_a = edgeBuf[index0].rpm;
float rpm_2 = edgeBuf[index+1].rpm;
float rpm_b = (rpm_2 - rpm_a)/TW_TOOTH_ALPHA * b_a + rpm_a;*/
float avgrpm = 0;
for(int i = index0; i < TW_PERCYL_TEETH + 1; i ++){ //poner security por overfkiw
avgrpm += edgeBuf[i].rpm;
}
avgrpm = avgrpm / nteeths;
//checkear si restando el tiempo de adquisicion al ic_ckp2
uint32_t diffCKP_1 = IC_CKP_Real >= edgeBuf[index].ic ? IC_CKP_Real - edgeBuf[index].ic : IC_CKP_Real - (-edgeBuf[index].ic - 0xffffffff);
uint32_t diffCKP_2 = edgeBuf[index+1].ic >= IC_CKP_Real ? edgeBuf[index+1].ic - IC_CKP_Real : edgeBuf[index+1].ic - (-IC_CKP_Real - 0xffffffff);
uint8_t toNext = diffCKP_1 > diffCKP_2 ? 1 : 0;
//uint32_t diffCKP = toNext ? diffCKP_2 : diffCKP_1;
uint32_t diffCKP = IC_CKP_Real >= edgeBuf[index0].ic ? IC_CKP_Real - edgeBuf[index0].ic : IC_CKP_Real - (-edgeBuf[index0].ic - 0xffffffff);
if(feedbackMT){
diffCKP = IC_CKP2 >= IC_MT ? IC_CKP2 - IC_MT : IC_CKP2 - (IC_MT - 0xffffffff);
}
//uint32_t diffCKP = IC_CKP2 >= IC_MT ? IC_CKP2 - IC_MT : IC_CKP2 - (IC_MT - 0xffffffff);
float diffbot = 0;
float difftop = 0;
if(safetySHUTOFF){
if(RPM < 150){
Timeout_StopByIndex(12); // Reset CKP timeout
}
}else{
if(count_CKP > 1){
if(feedbackMT){
fb_2 = MT_RPM * 6.0 * diffCKP / refClock - dFi;
}else{
fb_2 = avgrpm * 6.0 * diffCKP_e / refClock + (index0-1) * TW_TOOTH_ALPHA - dFi;
}
fb_2 -= FBKW_FEEDBACK_ZERO;
if(fb_2 > FBKW_FEEDBACK_MIN && fb_2 < FBKW_FEEDBACK_MAX){ //si esta entre margenes
new_fb = fb_2;
fb_lock = 0;
}else{
diffbot = FBKW_FEEDBACK_MIN - fb_2;
difftop = fb_2 - FBKW_FEEDBACK_MAX;
valid = 1;
}
}else{
//feedbackfirst = RPM * 6.0 * diffCKP / refClock;
if(feedbackMT){
fb_1 = ckp_rpm * 6.0 * diffCKP_e / refClock + (index0-1) * TW_TOOTH_ALPHA - dFi;
}else{
//fb_1 = (toNext ? -1 : 1) * edgeBuf[index + toNext].rpm * 6.0 * diffCKP / refClock + (teethCKP + toNext) * TW_TOOTH_ALPHA - dFi;
fb_1 = avgrpm * 6.0 * diffCKP_e / refClock + (index0-1) * TW_TOOTH_ALPHA - dFi;
}
fb_1 -= FBKW_FEEDBACK_ZERO;
if(fb_1 > FBKW_FEEDBACK_MIN && fb_1 < FBKW_FEEDBACK_MAX){
new_fb = fb_1;
fb_lock = 0;
valid = 1;
}else{
diffbot = FBKW_FEEDBACK_MIN - fb_1;
difftop = fb_1 - FBKW_FEEDBACK_MAX;
}
}
}
if(!valid && count_CKP > 1){ //si no encontro ningun otro nuevo valor dentro de rango, checkear boundaries
//estas diferencias son positivas cuando hay algun punto fuera.
if(difftop > 0 && diffbot > 0){
fb_lock = (difftop > diffbot) > 0 ? 2 : 1;
}else if(difftop > 0){
fb_lock = 2;
}else if(diffbot > 0){
fb_lock = 1;
}
}
if(fb_lock){ //si se ha encontrado bloqueo, set the value now
if(fb_lock > 1){
new_fb = FBKW_FEEDBACK_MAX;
}else{
new_fb = FBKW_FEEDBACK_MIN;
}
valid = 0;
}else{
Timeout_ResetByIndex(12, TIM16->CNT); // Reset CKP timeout
Timeout_ResetByIndex(13, TIM16->CNT);
//si estan fuera de bounds da error.
}
FBKW_FEEDBACK -= lpf_fb*(FBKW_FEEDBACK - new_fb - fbkw_offset);
ckp_process_pending=0;
}
void FBKW_RESET_FEEDBACK(){
//fb_1 = 0;
//fb_2 = 0;
//valid = 0;
count_CKP = 0;
uint8_t index0 = FBKW_FEEDBACK_ZERO / TW_TOOTH_ALPHA-1;
uint8_t nteeths = TW_PERCYL_TEETH - index0;
uint32_t diff_rpm_to_end = edgeBuf[TW_PERCYL_TEETH].ic >= edgeBuf[index0].ic ? edgeBuf[TW_PERCYL_TEETH].ic - edgeBuf[index0].ic : edgeBuf[TW_PERCYL_TEETH].ic - (-edgeBuf[index0].ic - 0xffffffff);
ckp_rpm = 60.0 * refClock / diff_rpm_to_end * (nteeths)/TW_THEETHS;
}
float real_Beta = 0.0;
extern float dPHI1_bip_nom;
void TW_CALC_REAL_BETA(){
uint8_t index = 0;
for(uint8_t i = 1; i < TW_PERCYL_TEETH; i++){
if(edgeBuf[i].ic < IC_INJ && IC_INJ < edgeBuf[i+1].ic){
index = i;
break;
}
}
if(!index){
return;
}
uint32_t Inj_Difference = edgeBuf[index + 1].ic >= IC_INJ ? edgeBuf[index+1].ic-IC_INJ : edgeBuf[index+1].ic-(IC_INJ-0xffffffff);
//uint32_t Inj_Difference = IC_INJ >= edgeBuf[index].ic ? IC_INJ-edgeBuf[index].ic : IC_INJ-(edgeBuf[index].ic-0xffffffff);
//uint32_t Tooth_Difference = edgeBuf[index+1].ic >= edgeBuf[index].ic ? edgeBuf[index+1].ic-edgeBuf[index].ic : edgeBuf[index+1].ic-(edgeBuf[index].ic-0xffffffff);
real_Beta = (index+1)*TW_TOOTH_ALPHA - 6.0*edgeBuf[index+1].rpm*Inj_Difference/refClock;
//real_Beta = index*TW_TOOTH_ALPHA + 6.0*edgeBuf[index].rpm*Inj_Difference/refClock;
/*float rpm_1 = edgeBuf[index].rpm;
float rpm_2 = edgeBuf[index+1].rpm;
float rpm_b = (rpm_2 - rpm_1)/(Tooth_Difference) * Inj_Difference + rpm_1;
real_Beta = index*TW_TOOTH_ALPHA + 6.0*rpm_b*Inj_Difference/refClock;*/
real_boi_pending = 0;
}
float correction_beta = 0.0;
volatile float s_boi_corr_deg = 0.0f;
float correction_boi_accel = 0.0;
float correction_boi_accel_2 = 0.0;
uint8_t boi_eval_pending = 0;
void TW_DEFER_BOI_EVAL(uint32_t ic){
IC_INJ = ic;
boi_eval_pending = 1;
}
uint8_t par = 0;
uint8_t filter = 4;
void TW_CALC_INJ_BOI(){
if(TW_RPM_SENSOR_STATE){
TW_UPDATE_BOI_COMP();
TW_UPDATE_INJ_DELAY();
}
correction_beta = s_boi_corr_deg + correction_boi_accel_2;
boi_eval_pending = 0;
}
extern uint32_t T_delay;
extern uint32_t T_delayFI;
float val;
void TW_UPDATE_INJ_DELAY(){
uint32_t t = TM_TIME_FROM_NEAREST_TOOTH(IC_INJ, 1);
if(t - (T_delayFI - T_delay) < 100){
T_delay = t - (T_delayFI - T_delay);
}else{
T_delay = 20;
}
float inj_rpm = TM_GET_RPM_FROM_NEAREST_TOOTH(IC_INJ, 0);
INJ_UPDATE_BOI_MARGIN(inj_rpm);
}
// Tunables (per update/event)
static float s_boi_Kp = 0.15f; // proportional gain
static float s_boi_Ki = 0.00f; // integral gain (per event)
static float s_boi_max_step_deg = 0.4f; // max per-update change
// State
static float s_boi_I = 0.0f; // integral accumulator
static float max_boi = 1.8;
void TW_UPDATE_BOI_COMP(void)
{
float err = PHI1 + dFi - actual_phi1;// + correction_boi_accel_2
// reject outliers, also clear integral so we don't carry stale windup
if (err > 4.0f || err < -4.0f) {
s_boi_I = 0.0f; return;
}
// Unsaturated control
float u_unsat = s_boi_Kp * err + s_boi_I;
// Saturate to per-step limit
float delta = u_unsat;
if (delta > s_boi_max_step_deg) delta = s_boi_max_step_deg;
if (delta < -s_boi_max_step_deg) delta = -s_boi_max_step_deg;
// Simple anti-windup: only integrate when not saturated
if (delta == u_unsat) {
s_boi_I += s_boi_Ki * err;
// optional clamp for the integral itself (keeps it reasonable)
/*float I_lim = s_boi_max_step_deg;
if (s_boi_I > I_lim) s_boi_I = I_lim;
if (s_boi_I < -I_lim) s_boi_I = -I_lim;*/
}
// Apply correction
s_boi_corr_deg += delta;
if (s_boi_corr_deg > max_boi) s_boi_corr_deg = max_boi;
if (s_boi_corr_deg < -max_boi) s_boi_corr_deg = -max_boi;
}
//static float accel_m = 0.23; //160 //comparing to last probamos con 5 en el coche si se reduce el error intentar dejarlo fino
//float accel_m_2=4; //antes
//ahora
static float accel_m = 0.35;
float accel_m_2=0;
float lastrpmteeth = 0.0;
float accel= 0;
float drpm_=0;
//float acc = 0;
//float lpf_accboi = 0.1;
uint8_t roundingmode;
extern float last_accel;
int tooth = 0;
void TW_CALCULATE_INSTANT_BOI_CORRECTION(){
//uint8_t lookuptooth = currentTooth < TW_PERCYL_TEETH ? currentTooth + 1 : 0;
//uint8_t dtooth = currentTooth >= lookuptooth ? currentTooth + 3 - lookuptooth : lookuptooth + TW_PERCYL_TEETH + 2 - (currentTooth);
uint8_t dtooth = 30;
//drpm_ = (TEETH_RPM - edgeBuf[lookuptooth].rpm)/(TEETH_RPM*dtooth);
/*if(MT_RPM > lastrpmteeth && MT_RPM > TEETH_RPM){
drpm_ = (TEETH_RPM - MT_RPM)/(TEETH_RPM*currentTooth);
}else if(MT_RPM < lastrpmteeth && MT_RPM < TEETH_RPM){
drpm_ = (TEETH_RPM - MT_RPM)/(TEETH_RPM*currentTooth);
}else{
//drpm_ = (TEETH_RPM - lastrpmteeth)/(TEETH_RPM*dtooth);
}*/
drpm_ = (TEETH_RPM - lastrpmteeth)/(dtooth);
/*if(drpm_ * last_accel < 20 ){
drpm_ = 0;
}*/
float drpm_2 = (MT_RPM - last_MT_RPM) / MT_RPM;
//acc -= lpf_accboi*(acc-drpm_);
//float drpm_ = (TEETH_RPM - lastrpmteeth)/TEETH_RPM;
float comp = drpm_ * accel_m + drpm_2 * accel_m_2;
//float comp = last_accel * accel_m;
//float comp = accel_m * TM_GET_LOCAL_ACC_TO_ANGLE(PHI1 + dFi, roundingmode);
//float comp = edgeBuf[currentTooth].accel * accel_m;
//correction_boi_accel_2 = (comp > 0.5 || comp < -0.5) ? 0 : comp ;
correction_boi_accel_2 = fclamp(comp, -1, 1);
/*if(correction_boi_accel_2 < -0.4){
tooth = currentTooth;
correction_boi_accel_2 = 0;
}*/
correction_beta = s_boi_corr_deg + correction_boi_accel_2;
}
void TW_PREPARE_BOI_ACCELCORRECTION(){
uint8_t index = 0;
for(uint8_t i = 1; i < TW_PERCYL_TEETH; i++){
if(edgeBuf[i].ic < IC_INJ && IC_INJ < edgeBuf[i+1].ic){
index = i;
break;
}
}
if(!index){
return;
}
lastrpmteeth = edgeBuf[index - 1].rpm;
}
float real_eoi = 0.0;
float correction_eoi = 0.0;
static volatile float s_eoi_corr_deg = 0.0f;
void TW_DEFER_EOI_EVAL(uint32_t ic){
IC_EOI = ic;
eoi_eval_pending = 1;
}
uint8_t inj_rpm_pending = 0;
void TW_CALC_REAL_EOI(){
real_eoi = TM_INTEGRATE_ANGLE_FROM_NEAREST_TOOTH(IC_EOI, 0); //friggin backwards integration
real_eoi_pending = 0;
real_eoi_updated = 1;
inj_rpm_pending = 1;
}
//float drpm_eoi = 0.0;
//float correction_eoi_accel = 0.0;
void TW_CALCULATE_INSTANT_EOI_CORRECTION(){
//float drpm_ = (TEETH_RPM - lastrpmteeth)/TEETH_RPM;
float valid_accel = last_accel;//fclamp(last_accel, -100, 50);
if(RPM < MIN_RPM){
return;
}
//float comp = fclamp(-valid_accel * eoi_acc_K / RPM, -10, 10);
//correction_eoi_accel += lpf_eoi_acc_k*(comp-correction_eoi_accel);
/*if(last_accel < 5 && last_accel > -5){
//correction_eoi_accel = 0;
//correction_eoi_accel += lpf_eoi_acc_k*(0-correction_eoi_accel);
}else{
correction_eoi_accel += lpf_eoi_acc_k*(comp-correction_eoi_accel);
}*/
//correction_eoi_accel = (comp > 2 || comp < -2) ? 0 : comp ;
//correction_eoi_accel = eoi_acc_K*correction_boi_accel_2;
//correction_eoi = correction_eoi_accel + s_eoi_corr_deg;
}
static inline float angle_diff_deg(float a, float b)
{
float d = a - b;
while (d > 180.0f) d -= 360.0f;
while (d < -180.0f) d += 360.0f;
return d;
}
static float s_eoi_Kp = 0.45f; // proportional gain
static float s_eoi_Ki = 0.00f; // integral gain (per event)
static float s_eoi_max_step_deg = 0.4f; // max per-update change
// State
static float s_eoi_I = 0.0f; // integral accumulator
extern float correction_eoi_accel;
float eoi_clamp = 1.67;//sacado del osciloscopio
float eoi_kslow = 0.45;
float cummulative_eoi_error = 0.0;
void TW_UPDATE_EOI_COMP(void)
{
float target = INJ_GET_TARGET_EOI();
float err = angle_diff_deg(target, real_eoi); // wrap-aware
/*if(!T_ein_status){
err = angle_diff_deg(target, TM_INTEGRATE_ANGLE_FROM_NEAREST_TOOTH(IC_EOI + 16*PH_PEAK_DEF,1));
}*/
// reject outliers, also clear integral so we don't carry stale windup
if (err > 30.0f || err < -30.0f) {
s_boi_I = 0.0f; return;
}
//estas dos lineas son nuevas, la idea es que se suma el nuevo error y se calcula en base a eso
err += cummulative_eoi_error;
err /= filter;
// Unsaturated control
/*if(err < -0.15 ){
err *= eoi_kslow;
}*/
float u_unsat = ((err < -0.15) ? eoi_kslow : s_eoi_Kp) * err + s_eoi_I;
// Saturate to per-step limit
float delta = u_unsat;
if (delta > s_eoi_max_step_deg) delta = s_eoi_max_step_deg;
if (delta < -s_eoi_max_step_deg) delta = -s_eoi_max_step_deg;
// Simple anti-windup: only integrate when not saturated
if (delta == u_unsat) {
s_eoi_I += s_eoi_Ki * err;
// optional clamp for the integral itself (keeps it reasonable)
/*float I_lim = s_boi_max_step_deg;
if (s_boi_I > I_lim) s_boi_I = I_lim;
if (s_boi_I < -I_lim) s_boi_I = -I_lim;*/
}
// Apply correction
s_eoi_corr_deg += delta;
s_eoi_corr_deg = fclamp (s_eoi_corr_deg, -eoi_clamp, eoi_clamp);
cummulative_eoi_error = 0;
}
void TW_UPDATE_EOI_ERROR(void)
{
float target = INJ_GET_TARGET_EOI();
float err = angle_diff_deg(target, real_eoi); // wrap-aware
// reject outliers, also clear integral so we don't carry stale windup
if (err > 30.0f || err < -30.0f) {
return;
}
cummulative_eoi_error += err;
}
void TW_CALC_INJ_EOI(){
if(TW_RPM_SENSOR_STATE && !blankInj){
if(!(par % filter)){
TW_UPDATE_EOI_COMP(); //igual haciendo promedios se subsana algo
}else{//este else es nuevo
TW_UPDATE_EOI_ERROR();
}
par++;
TM_UPDATE_END_DELAY_CORRECTION();
}
correction_eoi = s_eoi_corr_deg;
eoi_eval_pending = 0;
}
extern float T_ein;
extern float dPHI1_bip;
void TW_DEFER_TEIN_VAL(){
IC_NOM_BIP = IC_TEIN - TEIN_NOMINAL * 16; //16 prescaler
hasTeinDetEndedFlag = 1;
phi1_eval_pending = 1;
}
extern float dPHI1_bip_nom_2;
extern float dPHI1_bip_2;
void TW_CALC_BIP_ANGLE(){
IC_TEIN = IC_INJ + T_ein*16 - TEIN_READING_OFFSET*16; //16 prescaler
uint8_t index = 0;
for(uint8_t i = 1; i < TW_PERCYL_TEETH; i++){
if(edgeBuf[i].ic < IC_TEIN && IC_TEIN <= edgeBuf[i+1].ic){ //si lo llamo antes del siguiente diente nunca llega
index = i;
break;
}
}
if(!index){
return;
}
float newphi1 = TM_INTEGRATE_ANGLE_FROM_NEAREST_TOOTH(IC_TEIN, 0); //maybe it gets calculated fromtooth behing
if(newphi1 > 1){
actual_phi1 = newphi1;
phi1_eval_pending = 0;
real_bip_updated = 1;
}
/*if(actual_phi1 < 1){
index = 0;
}*/
//+TM_INTEGRATE_ANGLE_FROM_NEAREST_TOOTH(IC_TEIN, 0)
//actual_phi1 = TM_GET_REAL_BIP_ANGLE(IC_TEIN);
}
float fclamp(float value, float min, float max) {
const float t = value < min ? min : value;
return t > max ? max : t;
}
void TW_Reset_Pending_Tasks(){
boi_eval_pending = 0;
eoi_eval_pending = 0;
phi1_eval_pending = 0;
ckp_process_pending = 0;
}
void TW_Service(){
if(real_boi_pending){
TW_CALC_REAL_BETA();
}
if(real_eoi_pending){
TW_CALC_REAL_EOI();
}
if(boi_eval_pending && hasInjectionEnded){ //&& startedEngine
TW_CALC_INJ_BOI();
}
//antes estaba en vez de real eoi, con eoi eval, o sea apenas recibia eoi, pero sin actualizarlo??
if(eoi_eval_pending && hasInjectionEnded & real_eoi_updated){//&& startedEngine
TW_CALC_INJ_EOI();
}
if(phi1_eval_pending && hasTeinDetEnded){
TW_CALC_BIP_ANGLE();
}
if(real_eoi_updated && real_bip_updated && inj_rpm_pending){
TM_UPDATE_INJECTION_RPM_AND_ALPHA(real_eoi, actual_phi1, IC_EOI, IC_TEIN);
inj_rpm_pending = 0;
}
if(ckp_process_pending){
TW_CALC_FBKW_FEEDBACK();
}
}
Reference in New Issue View Git Blame Copy Permalink