179 lines
5.4 KiB
C
179 lines
5.4 KiB
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 UpdatePWM(TIM_HandleTypeDef* pwmHandle, uint32_t pwmChannel, uint16_t period_on, uint16_t period){
|
|
|
|
if(period_on > period)
|
|
{
|
|
period_on = period;
|
|
}
|
|
pwmHandle->Instance->ARR = period;
|
|
pwmHandle->Instance->CCR2 = period_on;
|
|
|
|
}
|
|
/* =======================================================================
|
|
* 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);
|
|
}
|
|
|
|
/* cl_gate_input (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_cl_gate_input(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.cl_gate_input = fbkw_get_cl_gate_input;
|
|
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);
|
|
}
|
|
|
|
uint8_t last_ckp = 1;
|
|
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) {
|
|
if(last_ckp){
|
|
//fbkw_rt.system_flags_110 = 0x30;
|
|
fbkw_rt.system_flags_110 |= 0x20u; // set ONLY bit 5 (OL gate). Leave other bits alone.
|
|
|
|
|
|
#if defined(T06211)
|
|
#elif defined(T06301)
|
|
fbkw_rt.mode_flags = 1;//muy importantes estas mierdas para que en open loop vaya como toca.
|
|
#endif
|
|
}
|
|
fbkw_rt.reset_flag = 1;
|
|
}else{
|
|
if(!last_ckp){
|
|
fbkw_rt.system_flags_110 &= ~0x20u; // clear ONLY bit 5 (OL gate). Leave other bits alone.
|
|
|
|
#if defined(T06211)
|
|
fbkw_rt.pi_open_loop_flag = 0x00; //0x2000
|
|
#endif
|
|
}
|
|
}
|
|
|
|
last_ckp = CKP_PULSE_AVAILABLE;
|
|
|
|
pwm_service(&fbkw_rt);
|
|
|
|
UpdatePWM(&htim4, TIM_CHANNEL_2, fbkw_rt.pwm_on_time, fbkw_rt.pwm_period);
|
|
FBKW_DC = ((float)fbkw_rt.pwm_duty) * (100.0f / 4095.0f);
|
|
FBKW_DEMAND = fbkw_rt.target_5e * (3.0f / 256.0f);
|
|
}
|
|
void FBKW_CKP_ISR(void) {
|
|
pwm_ckp_isr(&fbkw_rt);
|
|
}
|
|
|
|
const pwm_runtime_t *FBKW_pipeline_runtime(void) {
|
|
return &fbkw_rt;
|
|
}
|