hpsg5-controller_v2-stm32g4/Core/Advance_Control/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 FBKW_DEMAND = 0.0;
float FBKW_DC = 5;
float FBKW_FEEDBACK = 0.0;

float B_FB_KW = 0.0;

float DEMAND_filtered = 0.0;
extern float B_PHIAD;

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

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)));
}

/* =======================================================================
 * VP44 reverse-engineered pipeline bridge
 * -----------------------------------------------------------------------
 * The pipeline in pwm.c consumes 9 integer inputs through a getter vtable.
 * Each getter below converts from the project's native float/int variables
 * into the integer units the pipeline expects. Getters marked TODO return
 * zero until the correct source variable is wired up.
 *
 * Pipeline output `rt.pwm_duty` is a 12-bit (0..4095) duty command; it is
 * converted to a percentage and fed to UpdateFBKW_MODULATION for htim4/CH2.
 * =======================================================================
 */

static pwm_runtime_t        fbkw_rt;
static pwm_input_getters_t  fbkw_getters;

#define CF_KW 85.3333
#define CF_TMS 8.388
#define CF_ME 32
#define CF_T_M 16
#define CF_T_N 4368
#define CF_VOLT 54

/* ckp_in (0x02f8): sensed plunger position — fed by the CKP-derived
 * feedback. FBKW_FEEDBACK is already the plunger-position estimate in
 * this project. */
static int16_t fbkw_get_ckp_in(void *ctx) {
    (void)ctx;
    return (int16_t)(FBKW_FEEDBACK*CF_KW);
}

/* rpm (0x0040): engine RPM as uint16. */
static uint16_t fbkw_get_rpm(void *ctx) {
    (void)ctx;
    float r = RPM;
    if (r < 0.0f) r = 0.0f;
    if (r > 65535.0f) r = 65535.0f;
    return (uint16_t)(r*CF_TMS); //this is the value that uses the original, teeths/ms so 3degree n /ms.
}

/* angle_dec_cmd (0x0042): CAN angle-decrease command.
 * TODO: wire to the real source when available (was CAN-delivered on VP44). */
static int16_t fbkw_get_angle_dec_cmd(void *ctx) {
    (void)ctx;
    return (int16_t)(B_PHIAD*CF_KW);
}

/* inj_qty_demand (0x0044): injection quantity demand.
 * TODO: wire to project's injection-qty variable if/when exposed. */
static int16_t fbkw_get_inj_qty_demand(void *ctx) {
    (void)ctx;
    return (uint16_t)(ME*CF_ME);
}


/* b_fb_kw (0x02b4): KW plunger-feedback demand (baseline setpoint term).
 * Same semantic meaning as the project's B_FB_KW global. */
static int16_t fbkw_get_b_fb_kw(void *ctx) {
    (void)ctx;
    return (int16_t)(B_FB_KW*CF_KW);
}

/* state_130 (0x0130): CAN-delivered open/closed-loop discriminant.
 * TODO: wire when the corresponding CAN signal is exposed. Using
 * FBKW_PID_OPEN as a provisional mapping (negative = open-loop sentinel). */
static int16_t fbkw_get_state_130(void *ctx) {
    (void)ctx;
    return 0;
    //return (int16_t)(CKP_PULSE_AVAILABLE ? 0 : -1);

}

/* supply_voltage (0x0142): battery/supply voltage.
 * TODO: wire to the project's VBAT ADC reading. */
static uint16_t fbkw_get_supply_voltage(void *ctx) {
    (void)ctx;
    return (uint16_t)(736);//13.5 * its factor 54
}

/* temperature (0x0146): temperature input.
 * Temp is already a project-wide temperature variable (temperature.h). */
static int16_t fbkw_get_temperature(void *ctx) {
    (void)ctx;
    return (int16_t)(Temp*CF_T_M+CF_T_N);
}

void FBKW_init(void) {
    fbkw_getters.ckp_in         = fbkw_get_ckp_in;
    fbkw_getters.rpm            = fbkw_get_rpm;
    fbkw_getters.angle_dec_cmd  = fbkw_get_angle_dec_cmd;
    fbkw_getters.inj_qty_demand = fbkw_get_inj_qty_demand;
    fbkw_getters.b_fb_kw        = fbkw_get_b_fb_kw;
    fbkw_getters.state_130      = fbkw_get_state_130;
    fbkw_getters.supply_voltage = fbkw_get_supply_voltage;
    fbkw_getters.temperature    = fbkw_get_temperature;
    fbkw_getters.ctx            = NULL;

    pwm_init(&fbkw_rt, &pwm_cal_rom, &pwm_flash_rom, &fbkw_getters);
}

void FBKW_service(void) {
    /* Signal a reset edge to the supervisor when CKP pulses are not yet
     * available — matches the VP44 boot/CKP-lost reset behavior. */
    if (!CKP_PULSE_AVAILABLE) {
        fbkw_rt.reset_flag = 1;
        fbkw_rt.system_flags_110 = 0x30;
        fbkw_rt.mode_flags = 1;//muy importantes estas mierdas para que en open loop vaya como toca.
    }

    pwm_service(&fbkw_rt);

    /* pwm_duty is a 12-bit (0..4095) command. Convert to percentage for
     * the existing UpdateFBKW_MODULATION helper, which expects 0..100. */
    float duty_pct = ((float)fbkw_rt.pwm_duty) * (100.0f / 4095.0f);
    duty_pct = duty_pct < FBKW_PWM_MIN ? FBKW_PWM_MIN : duty_pct;
    duty_pct = duty_pct > FBKW_PWM_MAX ? FBKW_PWM_MAX : duty_pct;

    if (forceDC) {
        FBKW_DC = (float)forceDC;
    } else {
        FBKW_DC = duty_pct;
    }

    UpdateFBKW_MODULATION(&htim4, TIM_CHANNEL_2, FBKW_DC);
}

const pwm_runtime_t *FBKW_pipeline_runtime(void) {
    return &fbkw_rt;
}