hpsg5-controller-stm32g4/Core/Src/FBKW.c

/*
 * FBKW.c
 *
 *  Created on: Nov 20, 2024
 *      Author: herli
 */
#include <id.h>
#include "FBKW.h"
#include <stdint.h>
#include <math.h>
#include "injection.h"
#include "toothed_wheel.h"
#include "pre_injection.h"
#include "temperature.h"
/*
float errorFBKW = 0;
float error_sum = 0;
float prev_errorFBKW = 0;
float targetFBKW = 0;
float Kp = 7;		//Kp = 5, and Ki = 0.5
float Ki = 0.0005;
float Kd = 0.005;
float Ku = 1;
float Offset = 50.0;
float intFBKWerror = 0; 		//add += error * tstep
float Proportional = 0;
double Integrator = 0;
float derivative = 0;

float maxOutput = 45;
*/
float FBKW_DEMAND = 0.0;
float FBKW_DC = 5;
float FBKW_FEEDBACK = 0.0;

float B_FB_KW = 0.0;

float DEMAND_filtered = 0.0;

uint8_t CKP_PULSE_AVAILABLE = 0;
uint8_t FBKW_PID_OPEN = 1; //estaba en 0 antes, igual asi no se peta el pid si no recibe avance


uint8_t forceDC2 = 0;
void FBKW_PIDInterrupt(){
	UpdateFBKW_DEMAND(B_FB_KW);

	if(!TW_RPM_SENSOR_STATE){
		FBKW_DC = 5; //audi like this
	}
	else if(!CKP_PULSE_AVAILABLE || !timer1started){ 	//Hay timeout o aun no se hizo la primera inyección -> modo open loop
		UpdateFBKW_OpenDutyCycle(RPM);
		FBKW_PID_OPEN = 1;
	}else{
		UpdatePID(&myPID);
		FBKW_PID_OPEN = 0;
	}
	if(forceDC){
		FBKW_DC = forceDC; //onstart should be 21,8%.
	}
	/*if(forceDC2 > 0){
		FBKW_DC = forceDC2;
	}*/
	UpdateFBKW_MODULATION(&htim4, TIM_CHANNEL_2, FBKW_DC);//FBKW_DC
}

float F_clamp(float value, float min, float max) {
    if (value < min) {
        return min;
    } else if (value > max) {
        return max;
    } else {
        return value;
    }
}

float FBKW_DEMAND_OFFSET = -2.5;
float UpdateFBKW_DEMAND(float FBKW){
	if(FBKW < FBKW_MAX && FBKW > -FBKW_MAX){ //cuando envia 760, se tiene que quedar en el ultimo valor

		/*FBKW_DEMAND = FBKW > 90 ? 0 : FBKW * FBKW_DEM_M + FBKW_DEM_N; 	//el limite superior de 90 se respeta
		float variabledemand = fclamp(1.82+ 0.0024724*RPM - 0.2626, 0, 10);
		FBKW_DEMAND += variabledemand;*/

		float FBKW_DEM_0 = FBKW * FBKW_DEM_M;
		float FBKW_TEMP = FBKW_DEM_TEMP_N + FBKW_DEM_TEMP_M * Temp;

		float rpm_cor = RPM > 1 ? RPM / 1000 : 0;
		float FBKW_DEM_RPM = FBKW_DEM_A1*rpm_cor + FBKW_DEM_A2*rpm_cor*rpm_cor + FBKW_DEM_A3*rpm_cor*rpm_cor*rpm_cor;

		FBKW_DEMAND = FBKW_DEM_0 + FBKW_TEMP + FBKW_DEM_RPM;


#if HAS_PREINJECTION
		FBKW_DEMAND += FBKW_DEMAND_OFFSET * active_PI; //this is nasty
#endif

		FBKW_DEMAND = FBKW_DEMAND < FBKW_DEM_MIN ? FBKW_DEM_MIN : FBKW_DEMAND;
		DEMAND_filtered = FBKW_DEMAND;

		forceDC = 0;
		if(FBKW_DEMAND < FBKW_FEEDBACK + 1.66){
			Timeout_ResetByIndex(8, TIM16->CNT);  // Reset CKP timeout
		}
		return DEMAND_filtered;
	}/*else if(B_FB_NW == 0){ //esto siempre 0 porque no esta implementado
		FBKW_DC = FBKW_PWM_MIN;
		forceDC = FBKW_PWM_MIN;
		FBKW_DEMAND = 0;
		DEMAND_filtered = FBKW_DEMAND;
		return 0;
	}else{
		forceDC = 0;
		return FBKW_DEMAND;
	}*/
	else{
		forceDC = 0;
		//FBKW_DEMAND = 0;
		//DEMAND_filtered = FBKW_DEMAND;
		return FBKW_DEMAND;
	}

}
float UpdateFBKW_OpenDutyCycle(float RPM){
	if(RPM > 0){
		if(RPM > 200 && DEMAND_filtered <= 0){
			FBKW_DC = FBKW_PWM_MAX;
		}else{
			float a = 3.33*DEMAND_filtered-97.4;
			float b = -0.894*DEMAND_filtered+24.6;
			FBKW_DC = a+b*log(RPM);

			FBKW_DC = F_clamp(FBKW_DC, FBKW_PWM_MIN, FBKW_PWM_MAX);
		}
	}else{
		//FBKW_DC = 0;
	}
	return FBKW_DC;
}

void UpdateFBKW_MODULATION(TIM_HandleTypeDef* pwmHandle, uint32_t pwmChannel, float dutyCycle){
	//dutyCycle = F_clamp(dutyCycle, 5.0, 95.0);
	uint32_t newRegVal = (uint32_t)roundf((float)(pwmHandle->Instance->ARR) * (dutyCycle * 0.01f));

	/*In case of the dutyCycle being calculated as higher than the reload register, cap it to the reload register*/
	if(newRegVal > pwmHandle->Instance->ARR)
	{
		newRegVal = pwmHandle->Instance->ARR;
	}
	/*Assign the new dutyCycle count to the capture compare register.*/
	__HAL_TIM_SET_COMPARE(pwmHandle, pwmChannel, (uint32_t)(roundf(newRegVal)));
}

void definePID(struct PID *pid, float Kp, float Ki, float Kd, float Kaw, float Bias, float T_C, float T, float max, float min, float max_rate, float integral, float err_prev, float deriv_prev, float command_sat_prev, float command_prev){
	myPID.Kp = Kp;
	myPID.Ki = Ki;
	myPID.Kd = Kd;
	myPID.Kaw = Kaw;
	myPID.Bias = Bias;
	myPID.T_C = T_C;			// Time constant for derivative filtering
	myPID.T = T;				// Time step
	myPID.max = max;
	myPID.min = min;
	myPID.max_rate = max_rate;
	myPID.integral = integral;
	myPID.err_prev = err_prev;
	myPID.deriv_prev = deriv_prev;
	myPID.command_sat_prev = command_sat_prev;
	myPID.command_prev = command_prev;
}
void initPID(struct PID *pid, float T_C, float T){
	myPID.Kp = FBKW_PID_KP;
	myPID.Ki = FBKW_PID_KI;
	myPID.Kd = FBKW_PID_KD;
	myPID.Kaw = FBKW_PID_KAW;
	myPID.Bias = FBKW_PID_BIAS;
	myPID.T_C = T_C;			// Time constant for derivative filtering
	myPID.T = T;				// Time step
	myPID.max = FBKW_PWM_MAX;
	myPID.min = FBKW_PWM_MIN;
	myPID.max_rate = FBKW_PID_MAXRATE;
	myPID.integral = FBKW_PID_INTEGRAL;
	myPID.err_prev = 0;
	myPID.deriv_prev = 0;
	myPID.command_sat_prev = 0;
	myPID.command_prev = 0;
}

void updatePIDfreq(struct PID *pid, uint8_t millisPID){
	TIM6->ARR = millisPID * 1000;
	myPID.T = 1.0*millisPID/1000;				// Time step
	//derivative filtering time constant update not available
}

void UpdatePID(struct PID *pid)
{
    /* This function implements a PID controller.
     *
     * Inputs:
     *   measurement: current measurement of the process variable
     *   setpoint: desired value of the process variable
     *   pid: a pointer to a PID struct containing the controller parameters
     *
     * Returns:
     *   command_sat: the control output of the PID controller (saturated based on max. min, max_rate)
     */

    float err;
    float command;
    float command_sat;
    float deriv_filt;

    /* Error calculation */
    //err = FBKW_FEEDBACK - DEMAND_filtered;
    err = FBKW_FEEDBACK - FBKW_DEMAND;

    //err = FBKW_DEMAND - FBKW_FEEDBACK;

    /* Integral term calculation - including anti-windup */
    pid->integral += pid->Ki*err*pid->T + pid->Kaw*(pid->command_sat_prev - pid->command_prev)*pid->T;

    /* Derivative term calculation using filtered derivative method */
    deriv_filt = (err - pid->err_prev + pid->T_C*pid->deriv_prev)/(pid->T + pid->T_C);
    pid->err_prev = err;
    pid->deriv_prev = deriv_filt;

    /* Summing the 3 terms */
    command = pid->Kp*err + pid->integral + pid->Kd*deriv_filt + pid->Bias;

    /* Remember command at previous step */
    pid->command_prev = command;

    /* Saturate command */
    if (command > pid->max)
    {
        command_sat = pid->max;
    }
    else if (command < pid->min)
    {
        command_sat = pid->min;
    }
    else
    {
        command_sat = command;
    }

    /* Apply rate limiter */
    if (command_sat > pid->command_sat_prev + pid->max_rate*pid->T)
    {
        command_sat = pid->command_sat_prev + pid->max_rate*pid->T;
    }
    else if (command_sat < pid->command_sat_prev - pid->max_rate*pid->T)
    {
        command_sat = pid->command_sat_prev - pid->max_rate*pid->T;
    }
    else
    {
        /* No action */
    }

    /* Remember saturated command at previous step */
    pid->command_sat_prev = command_sat;
    FBKW_DC = command_sat;
    //return command_sat;
}