Firmware/hpsg5-controller-stm32g4
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
3770b8df49398c26ed2dac67e8d887eae1511748
hpsg5-controller-stm32g4/Core/Src/timing_manager.c

1060 lines
32 KiB
C
Raw Blame History

/*
* timing_manager.c
*
* Created on: Oct 3, 2025
* Author: herli
*/
#include "timing_manager.h"
#include "toothed_wheel.h"
#include "tein_detection.h"
#include "injection.h"
#include "main.h"
volatile EdgeSample edgeBuf[TW_PERCYL_TEETH+1];
/*#define MT_BUF_SZ 16
typedef struct { float rpm; float acc; } MTSample;
static MTSample mtBuf[MT_BUF_SZ];
static uint8_t mtHead = 0; // points to newest*/
float last_INJECTING_RPM = 0.0;
float last_non_Inj_RPM = 0.0;
float last_INJECTING_ALPHA = 0.0;
float last_accel = 0.0;
//static inline void MT_Push(float rpm_now, float sector_deg);
//static inline float rpm_acc_from_sector(float rpm_prev, float rpm_now, float sector_deg);
static inline float MT_AccelUpdate(float rpm_now);
static inline void MT_AccelReset();
static inline float PredictRPM(float rpm0, float accel_rpms, float dt_s);
static inline float TimeToAngleDeg(float rpm0, float accel_rpms, float dtheta_deg);
static inline float _rpm_from_dt(float deg, uint32_t dt_ticks);
static inline uint32_t ticks_diff(uint32_t newer, uint32_t older);
static inline uint32_t ticks_diff(uint32_t newer, uint32_t older)
{
return (newer >= older) ? (newer - older) : (newer + (0xFFFFFFFFu - older) + 1u);
}
static inline uint8_t TM_GET_IN_BUFFER_ID(uint32_t C){
uint8_t index = 0;
for(uint8_t i = 1; i < TW_PERCYL_TEETH; i++){
if(edgeBuf[i].ic < C && C < edgeBuf[i+1].ic){
index = i;
break;
}
}
return index;
}
void UpdateEdgeBuffer(uint32_t tickNew, float rpm_est, uint8_t tooth, uint8_t isInj){
if(tooth > TW_PERCYL_TEETH){
return;
}
EdgeSample s;
s.ic = tickNew;
s.rpm = rpm_est;
s.isInj = isInj;
//uint32_t accel_Difference = tickNew >= edgeBuf[tooth].ic ? tickNew-edgeBuf[tooth].ic : tickNew-(edgeBuf[tooth].ic-0xffffffff);
//float time = accel_Difference / refClock;
float time = ticks_diff(tickNew, edgeBuf[tooth].ic) / refClock;
//s.accel = USTODEG*(rpm_est - edgeBuf[tooth].rpm)/time; //in º/us^^2
s.accel = (rpm_est - edgeBuf[tooth].rpm)/time; //in º/º^^2
s.new = 1;
edgeBuf[tooth] = s;
}
void ClearEdgeBuffer(){
EdgeSample s;
s.ic = 0;
s.rpm = 0;
s.accel = 0;
s.isInj = 0;
s.new = 0;
for(int i = 0; i<TW_PERCYL_TEETH; i++){
edgeBuf[i] = s;
}
}
void MakeEdgeBufferOld(){
for(int i = 0; i<TW_PERCYL_TEETH; i++){
edgeBuf[i].new = 0;
}
}
float TM_GET_RPM_FROM_NEAREST_TOOTH(uint32_t C_Ticks, uint8_t fw){
uint8_t idx = TM_GET_IN_BUFFER_ID(C_Ticks);
if(!idx){return 0;}
return edgeBuf[idx + !fw].rpm;
}
float TM_TIME_FROM_NEAREST_TOOTH(uint32_t C_Ticks, uint8_t fw){
uint8_t idx = TM_GET_IN_BUFFER_ID(C_Ticks);
if(!idx){return 0;}
idx += 1 - fw;
uint32_t C_Difference = C_Ticks >= edgeBuf[idx].ic ? C_Ticks-edgeBuf[idx].ic : C_Ticks-(edgeBuf[idx].ic-0xffffffff);
return C_Difference / 16;
}
static inline float _angle_from_dt(float rpm, uint32_t dt_ticks)
{
// deg = 6 * rpm * seconds ; seconds = dt_ticks / refClock
return 6.0f * rpm * ((float)dt_ticks / (float)refClock);
}
static inline float _rpm_from_dt(float deg, uint32_t dt_ticks)
{
// deg = 6 * rpm * seconds ; seconds = dt_ticks / refClock
return deg / (6.0f * ((float)dt_ticks / (float)refClock));
}
float TM_INTEGRATE_ANGLE_FROM_NEAREST_TOOTH(uint32_t C_Ticks, uint8_t fw)
{
float int_angle = 0.0f;
uint8_t idx = TM_GET_IN_BUFFER_ID(C_Ticks);
if (!idx) return 0.0f;
else if (fw == 1) {
// from this tooth edge up to C_Ticks
uint32_t dt = (uint32_t)(C_Ticks - edgeBuf[idx].ic); // wrap-safe
float rpm = edgeBuf[idx].rpm;
float ddeg = _angle_from_dt(rpm, dt);
// angle = tooth base + fraction forward in this tooth
int_angle = (float)idx * TW_TOOTH_ALPHA + ddeg;
} else {
// from next tooth edge down to C_Ticks
uint8_t nx = (uint8_t)(idx + 1);
if (nx > TW_PERCYL_TEETH) return 0.0f;
uint32_t dt = (uint32_t)(edgeBuf[nx].ic - C_Ticks); // wrap-safe
float rpm = edgeBuf[nx].rpm; // rpm of the tooth we are "in"
float ddeg = _angle_from_dt(rpm, dt);
// angle = next tooth base minus the fraction we rewound
int_angle = (float)nx * TW_TOOTH_ALPHA - ddeg;
}
return int_angle;
}
float INTEGRATE_ANGLE_FROM_REFERENCE(float time, float reference, uint8_t fw){
float int_angle = 0.0f;
float rem_time = time;
uint8_t idx = reference / TW_TOOTH_ALPHA;
float off_angle = reference - idx * TW_TOOTH_ALPHA;
if (!idx) return 0.0f;
/*else if (fw == 1) {
for(int i = idx; i < TW_TOOTH_ALPHA; i--){
float dt_tooth = ticks_diff(edgeBuf[i].ic, edgeBuf[i-1].ic)/16;
if(i == nx){ //si es el primer valor (va a ser parcial)
float alpha_dt = off_angle / TW_TOOTH_ALPHA * dt_tooth; // wrap-safe
if(alpha_dt > rem_time){
float diff = alpha_dt - rem_time;
int_angle += off_angle - TW_TOOTH_ALPHA * diff/dt_tooth;
rem_time = 0;
}else{
int_angle += off_angle;
rem_time -= alpha_dt;
}
}else if(rem_time > dt_tooth){
int_angle += TW_TOOTH_ALPHA;
rem_time -= dt_tooth;
}else{
int_angle += TW_TOOTH_ALPHA * rem_time/dt_tooth;
rem_time = 0;
}
if(rem_time < 0.5f){
break;
}
}*/
//} else {
// from next tooth edge down to C_Ticks
uint8_t nx = (uint8_t)(idx + 1);
if (nx > TW_PERCYL_TEETH) return 0.0f;
for(int i = nx; i > 1; i--){
float dt_tooth = ticks_diff(edgeBuf[i].ic, edgeBuf[i-1].ic)/16;
if(i == nx){ //si es el primer valor (va a ser parcial)
float alpha_dt = off_angle / TW_TOOTH_ALPHA * dt_tooth; // wrap-safe
if(alpha_dt > rem_time){
float diff = alpha_dt - rem_time;
int_angle += off_angle - TW_TOOTH_ALPHA * diff/dt_tooth;
rem_time = 0;
}else{
int_angle += off_angle;
rem_time -= alpha_dt;
}
}else if(rem_time > dt_tooth){
int_angle += TW_TOOTH_ALPHA;
rem_time -= dt_tooth;
}else{
int_angle += TW_TOOTH_ALPHA * rem_time/dt_tooth;
rem_time = 0;
}
if(rem_time < 0.5f){
break;
}
}
//}
return int_angle;
}
float TM_GET_REAL_BIP_ANGLE(uint32_t C_Ticks)
{
float int_angle = 0.0f;
uint8_t idx = TM_GET_IN_BUFFER_ID(C_Ticks);
if (!idx) return 0.0f;
uint8_t nx = (uint8_t)(idx + 1);
if (nx > TW_PERCYL_TEETH) return 0.0f;
uint32_t t0_ticks = edgeBuf[idx].ic;
uint32_t t1_ticks = edgeBuf[nx].ic;
uint32_t Dt_ticks = ticks_diff(t1_ticks, t0_ticks);
float Dt = Dt_ticks/refClock;
// Base angular velocities (deg/s)
float w0 = edgeBuf[idx].rpm*6.0f;
float w1 = edgeBuf[nx].rpm*6.0f;
// Scale both w0,w1 so the *area* over the full tooth equals TW_TOOTH_ALPHA
// Integral of linear ω over [0,Dt] is 0.5*(w0 + w1)*Dt
float denom = 0.5f * (w0 + w1) * Dt;
float s = (denom != 0.0f) ? (TW_TOOTH_ALPHA / denom) : 0.0f;
float w0s = w0 * s;
float w1s = w1 * s;
float a_s = (w1s - w0s) / Dt;
//if (fw == 1) {
// Forward from t0 to C_Ticks
uint32_t dt_ticks = ticks_diff(C_Ticks, t0_ticks);
if (dt_ticks > Dt_ticks) dt_ticks = Dt_ticks; // clamp inside tooth
float dt = (dt_ticks)/refClock;
float theta_local = w0s * dt + 0.5f * a_s * dt * dt; // deg
int_angle = (float)idx * TW_TOOTH_ALPHA + theta_local;
/*} else {
// Backward from t1 down to C_Ticks
uint32_t dt_ticks = u32_diff(t1_ticks, C_Ticks);
if (dt_ticks > Dt_ticks) dt_ticks = Dt_ticks; // clamp
float dt = TICKS_TO_SECONDS(dt_ticks);
// Integrate backward: area from (t1 - dt) to t1 equals using end-velocity w1s
// Symmetry gives: θ_local = w1s*dt - 0.5*a_s*dt^2
float theta_local = w1s * dt - 0.5f * a_s * dt * dt; // deg
int_angle = (float)nx * TW_TOOTH_ALPHA - theta_local;
}*/
// Optional: keep in [0, total_per_cycle) if you need it
return int_angle;
}
float TM_INTEGRATE_ANGLE_FROM_REFERENCE(uint32_t C_time, float refAngle, uint8_t fw){
/*float int_angle = 0.0;
uint8_t idx = TM_GET_IN_BUFFER_ID(C_Ticks);
if(!idx){
return int_angle;
}
idx = fw ? idx : idx + 1;
if(idx > TW_PERCYL_TEETH){
return int_angle;
}*/
float int_angle = refAngle;
uint8_t id_ref = refAngle / TW_TOOTH_ALPHA;
id_ref = fw ? id_ref : id_ref + 1;
while(C_time){
id_ref = int_angle / TW_TOOTH_ALPHA + !fw*1;
int_angle += -1*!fw*edgeBuf[id_ref].rpm*USTODEG;
C_time--;
}
/*if(fw){
uint32_t C_Difference = C_Ticks >= edgeBuf[idx].ic ? C_Ticks-edgeBuf[idx].ic : C_Ticks-(edgeBuf[idx].ic-0xffffffff);
int_angle = idx*TW_TOOTH_ALPHA + 6.0*edgeBuf[idx].rpm*C_Difference/refClock;
}else{
uint32_t C_Difference = edgeBuf[idx].ic >= C_Ticks ? edgeBuf[idx].ic-C_Ticks : edgeBuf[idx].ic-(C_Ticks-0xffffffff);
int_angle = idx*TW_TOOTH_ALPHA - 6.0*edgeBuf[idx].rpm*C_Difference/refClock;
}*/
//int_angle = fw ? int_angle - refAngle : int_angle - refAngle;
return int_angle;
}
float dalpha;
float TM_INTEGRATE_TIME_FROM_REFERENCE(float C_Angle, uint8_t teeth_f){
float int_time = 0.0;
uint8_t idx = C_Angle/TW_TOOTH_ALPHA; //empiezo diente despues de phi1
if(idx < teeth_f){
return 0.0f;
}
float C_Difference = C_Angle - idx*TW_TOOTH_ALPHA;
int_time = C_Difference/(edgeBuf[idx+1].rpm*USTODEG);
for(int i = idx; i > teeth_f; i--){
if(edgeBuf[i].rpm < 20){
break;
}
int_time += TW_TOOTH_ALPHA / (edgeBuf[i].rpm*USTODEG);
}
return int_time;
}
uint8_t accelbipenabled = 0;
float TM_INTEGRATE_TIME_BIP_PREDICTIVE(float C_Angle, uint8_t teeth_i){
float int_time = 0.0;
uint8_t idx = C_Angle/TW_TOOTH_ALPHA; //empiezo diente despues de phi1
if(idx < teeth_i){
return 0.0f;
}
float Remaining_Angle = C_Angle - idx*TW_TOOTH_ALPHA;
uint8_t lastUpdatedTeeth = 0;
for(int i = 1; i < idx + 1; i++){
if(edgeBuf[i].new){
lastUpdatedTeeth = i;
break;
}
}
if(!lastUpdatedTeeth || lastUpdatedTeeth > TW_PERCYL_TEETH){
return TM_INTEGRATE_TIME_BIP(C_Angle, teeth_i);
}
float acc_factor_f = edgeBuf[lastUpdatedTeeth].rpm / edgeBuf[idx-1].rpm; //cuanto cambio desde la ultima
float acc_factor_s = edgeBuf[idx+1].rpm / edgeBuf[idx-1].rpm; //cuanto se desacelera en la inyeccion
acc_factor_f = accelbipenabled ? acc_factor_f : 1.0;
acc_factor_s = accelbipenabled ? acc_factor_s : 1.0;
float rpm = edgeBuf[lastUpdatedTeeth].rpm * acc_factor_s > MIN_RPM ? edgeBuf[lastUpdatedTeeth].rpm * acc_factor_s : TEETH_RPM;
int_time = Remaining_Angle / (rpm*USTODEG);
for(int i = teeth_i; i < idx; i++){
if(edgeBuf[i+1].rpm < MIN_RPM){
rpm = TEETH_RPM;
}else{
rpm = edgeBuf[i+1].isInj ? edgeBuf[lastUpdatedTeeth].rpm * acc_factor_s : edgeBuf[i+1].rpm * acc_factor_f;
}
int_time += TW_TOOTH_ALPHA / (rpm*USTODEG);
}
return int_time;
}
float correction_factor;
float bip_acc_K = 0;
float TM_INTEGRATE_TIME_BIP(float C_Angle, uint8_t teeth_i){
float int_time = 0.0;
uint8_t idx = C_Angle/TW_TOOTH_ALPHA; //empiezo diente despues de phi1
if(idx < teeth_i){
return 0.0f;
}
float Remaining_Angle = C_Angle - idx*TW_TOOTH_ALPHA;
//float rpm = edgeBuf[idx+1].rpm > MIN_RPM ? edgeBuf[idx+1].rpm : TEETH_RPM;
//int_time = Remaining_Angle / (rpm*USTODEG);
int_time = Remaining_Angle/TW_TOOTH_ALPHA * ticks_diff(edgeBuf[idx+1].ic, edgeBuf[idx].ic)/16;
for(int i = teeth_i; i < idx; i++){
int_time += ticks_diff(edgeBuf[i+1].ic, edgeBuf[i].ic)/16;
}
/* acceleration factor */
float dt_to_bip = TimeToAngleDeg(MT_RPM, last_accel, C_Angle);
float rpm_at_bip = PredictRPM(MT_RPM, last_accel, dt_to_bip);
correction_factor = fclamp(MT_RPM/rpm_at_bip,1,1.5);
int_time *= 1 + (correction_factor-1)*bip_acc_K;
return int_time;
}
/*float TM_INTEGRATE_TIME_BIP(float C_Angle, uint8_t teeth_i){
float int_time = 0.0;
uint8_t idx = C_Angle/TW_TOOTH_ALPHA; //empiezo diente despues de phi1
if(idx < teeth_i){
return 0.0f;
}
float Remaining_Angle = C_Angle - idx*TW_TOOTH_ALPHA;
float rpm = edgeBuf[idx+1].rpm > MIN_RPM ? edgeBuf[idx+1].rpm : TEETH_RPM;
int_time = Remaining_Angle / (rpm*USTODEG);
for(int i = teeth_i; i < idx; i++){
rpm = edgeBuf[i+1].rpm > MIN_RPM ? edgeBuf[i+1].rpm : TEETH_RPM;
int_time += TW_TOOTH_ALPHA / (rpm*USTODEG);
}
return int_time;
}*/
uint8_t TM_GET_TRIGGER_TEETH_FROM_REFERENCE_AND_TIME(float C_Angle, float time){
float int_time = 0.0;
uint8_t idx = C_Angle/TW_TOOTH_ALPHA; //empiezo diente despues de phi1
if(idx > TW_PERCYL_TEETH){
return 0;
}
float C_Difference = C_Angle - idx*TW_TOOTH_ALPHA;
int_time = C_Difference/(edgeBuf[idx+1].rpm*USTODEG);
uint8_t idx_trigger = 0;
for(int i = idx; i > 0; i--){
if(edgeBuf[i].rpm < 20){
break;
}
int_time += TW_TOOTH_ALPHA / (edgeBuf[i].rpm*USTODEG);
if(int_time > time){
return i; //seria -1 pero luego le restamos, asi puede encontrar el 1 tmb;
break;
}
}
return 0;
}
uint8_t dtein_mode = 13;
float dtein_rpm = 0;
float extra = 0;
float extra2 = 00;
extern float eq_rpm;
float rpmdif = 100;
int negative = 0;
float TM_GET_PHIAD_dTEIN(float EOI_Angle, float MT_RPM){
float d_alpha = 0.0f;
uint8_t idx = EOI_Angle/TW_TOOTH_ALPHA; //empiezo diente despues de phi1
float Remaining_Angle = EOI_Angle - idx*TW_TOOTH_ALPHA;
float rpm0 = MT_RPM; // your base RPM
float accel = last_accel; // RPM/s from your filter
if(accel > -200 && accel < 200){
// A) Adjust a time computed from an angle (e.g., 31° to EOI)
float dt_to_eoi = TimeToAngleDeg(rpm0, accel, EOI_Angle);
// B) Predict the RPM at that firing time, if you need it
float rpm_at_eoi = PredictRPM(rpm0, accel, dt_to_eoi);
//d_alpha = - (TEIN_NOMINAL+!T_ein_status*PH_PEAK_DEF)*USTODEG*rpm_at_eoi;
d_alpha = - (TEIN_NOMINAL+!T_ein_status*PH_PEAK_DEF)*USTODEG*MT_RPM;
}else{
d_alpha = - (TEIN_NOMINAL+!T_ein_status*PH_PEAK_DEF)*USTODEG*MT_RPM;
}
//no compensations
float rpm = 0;
float rpm_inj_sum = 0.0f;
uint8_t n_inj = 0;
switch(dtein_mode){ //something between 1 and 2, but non are working properly
case 0:
rpm = MT_RPM;
break;
case 1:
rpm = edgeBuf[idx].rpm;
break;
case 2:
rpm = TEETH_RPM;
break;
case 3:
// --- 2) Build avg RPM pools (slowed vs free) ---
for (int i = 0; i < TW_PERCYL_TEETH; i++) {
float r = edgeBuf[i].rpm;
if (r < 20.0f) continue;
if (edgeBuf[i].isInj) { rpm_inj_sum += r; n_inj++; }
}
rpm = n_inj ? (rpm_inj_sum / n_inj) : TEETH_RPM;
break;
case 4:
for (int i = 1; i < TW_PERCYL_TEETH+1; i++) {
float r = edgeBuf[i].rpm;
if (r < 20.0f) continue;
rpm_inj_sum += r;
n_inj++;
}
//rpm = n_inj ? (rpm_inj_sum / n_inj) : TEETH_RPM;
rpm = n_inj ? ((rpm_inj_sum + extra * edgeBuf[TW_PERCYL_TEETH].rpm) / (n_inj + extra)) : TEETH_RPM;
break;
case 5:
for (int i = 1; i < TW_PERCYL_TEETH; i++) {
float r = edgeBuf[i].rpm;
if (r < 20.0f) continue;
rpm_inj_sum += r; n_inj++;
if (edgeBuf[i].isInj) { break; }
}
rpm = n_inj ? (rpm_inj_sum / n_inj) : TEETH_RPM;
break;
case 6:
for (int i = 1; i < TW_PERCYL_TEETH; i++) {
float r = edgeBuf[i].rpm;
if (r < 20.0f) continue;
rpm_inj_sum += r; n_inj++;
if (edgeBuf[i].isInj && !edgeBuf[i+1].isInj) { break; }
}
rpm = n_inj ? (rpm_inj_sum / n_inj) : TEETH_RPM;
break;
case 7:
// --- 2) Build avg RPM pools (slowed vs free) ---
uint8_t flag = 0;
for (int i = 1; i < TW_PERCYL_TEETH; i++) {
float r = edgeBuf[i].rpm;
if (r < 20.0f) continue;
if (edgeBuf[i].isInj) { flag = 1; }
if(flag){
rpm_inj_sum += r; n_inj++;
}
}
rpm = n_inj ? (rpm_inj_sum / n_inj) : TEETH_RPM;
break;
case 8:
// --- 2) Build avg extended RPM pools (slowed vs free) ---
for (int i = 0; i < TW_PERCYL_TEETH; i++) {
float r = edgeBuf[i].rpm;
if (r < 20.0f) continue;
if (edgeBuf[i].isInj){
rpm_inj_sum += r; n_inj++;
}else if(edgeBuf[i-1].isInj) { rpm_inj_sum += r; n_inj++; }
}
rpm = n_inj ? (rpm_inj_sum / n_inj) : TEETH_RPM;
break;
case 9:
for (int i = 1; i < idx + 2; i++) {
float r = edgeBuf[i].rpm;
if (r < 20.0f) continue;
if (edgeBuf[i].isInj){
rpm_inj_sum += r; n_inj++;
}else if(edgeBuf[i-1].isInj) { rpm_inj_sum += r; n_inj++; }
}
rpm = n_inj ? (rpm_inj_sum / n_inj) : TEETH_RPM;
break;
case 10:
uint32_t ticks_eoi = edgeBuf[idx].ic + Remaining_Angle/TW_TOOTH_ALPHA * ticks_diff(edgeBuf[idx+1].ic, edgeBuf[idx].ic) - TEIN_NOMINAL*16;
uint8_t id_end = TM_GET_IN_BUFFER_ID(ticks_eoi);
if(id_end){
return TM_INTEGRATE_ANGLE_FROM_NEAREST_TOOTH(ticks_eoi, 0)- EOI_Angle;
//float angle = 1.0f * TW_TOOTH_ALPHA * ticks_diff(ticks_eoi, edgeBuf[id_end].ic) / ticks_diff(edgeBuf[id_end+1].ic, edgeBuf[id_end].ic);
//return -(angle + TW_TOOTH_ALPHA*(idx - id_end - 1) + Remaining_Angle);
}
//uint32_t ic_nom_eoi = IC_EOI + TEIN_NOMINAL*16;
break;
case 11:
for (int i = 1; i < TW_PERCYL_TEETH+1; i++) {
float r = edgeBuf[i].rpm;
if (r < 20.0f) continue;
rpm_inj_sum += r;
n_inj++;
}
rpm = n_inj ? (rpm_inj_sum / n_inj) : TEETH_RPM;
float dt_to_eoi = TimeToAngleDeg(MT_RPM, last_accel, EOI_Angle); //tendria q ser mt rpm no las last injecting rpm.
// B) Predict the RPM at that firing time, if you need it
float rpm_next = PredictRPM(rpm0, last_accel, dt_to_eoi);
rpm *= rpm_next / MT_RPM;
break;
case 12:
// A) Adjust a time computed from an angle (e.g., 31° to EOI)
float dt_to_eoi2 = TimeToAngleDeg(MT_RPM, accel, 360/CYLINDERS);//EOI_Angle
// B) Predict the RPM at that firing time, if you need it
float rpm_at_eoi = fclamp(PredictRPM(MT_RPM, last_accel, dt_to_eoi2),MT_RPM-rpmdif,MT_RPM+rpmdif);
//if((rpm_at_eoi - MT_RPM)*last_accel > 0){
rpm = rpm_at_eoi;
break;
//}else{
// rpm = MT_RPM;
//}
case 13:
for (int i = 1; i < TW_PERCYL_TEETH+1; i++) {
float r = edgeBuf[i].rpm;
if (r < 20.0f) continue;
rpm_inj_sum += r;
n_inj++;
}
//rpm = n_inj ? (rpm_inj_sum / n_inj) : TEETH_RPM;
rpm = n_inj ? ((rpm_inj_sum + extra * edgeBuf[TW_PERCYL_TEETH].rpm) / (n_inj + extra)) : TEETH_RPM;
// A) Adjust a time computed from an angle (e.g., 31° to EOI)
float dt_to_eoi3 = TimeToAngleDeg(rpm, accel, extra2);//EOI_Angle //360/CYLINDERS
if (negative){ dt_to_eoi3 *= -1;}
// B) Predict the RPM at that firing time, if you need it
float rpm_at_eoi2 = fclamp(PredictRPM(rpm, last_accel, dt_to_eoi3),rpm-rpmdif,rpm+rpmdif);
//if((rpm_at_eoi - MT_RPM)*last_accel > 0){
rpm = rpm_at_eoi2;
//}else{
// rpm = MT_RPM;
//}
break;
default:
break;
}
dtein_rpm = rpm;
d_alpha = - (TEIN_NOMINAL)*USTODEG*dtein_rpm; //bien, usa las slowed -!T_ein_status*PH_PEAK_DEF*USTODEG*TEETH_RPM
if(!T_ein_status){
//d_alpha -= INTEGRATE_ANGLE_FROM_REFERENCE(PH_PEAK_DEF, EOI_Angle + d_alpha, 0);
d_alpha -= PH_PEAK_DEF*USTODEG*dtein_rpm ;
}
return d_alpha;
}
uint8_t current_inj_rpm_available = 0;
float inj_accel;
//UNUSED
float TM_INTEGRATE_TIME_FROM_REFERENCE_Forward(float C_Angle, uint8_t teeth_i, float InitInj){
float int_time = 0.0;
uint8_t idx = C_Angle/TW_TOOTH_ALPHA; //empiezo diente despues de phi1
if(idx < teeth_i){
return 0.0f;
}
float Remaining_Angle = C_Angle - idx*TW_TOOTH_ALPHA;
/* calcular la current accel con MT_RPM hasta teeth rpm (solo vale antes de empezar a inyectar) y comparar signo*/
/*uint32_t dt = (uint32_t)(edgeBuf[trigger_teeth].ic - edgeBuf[1].ic); // wrap-safe
float rpm_now = _rpm_from_dt(trigger_teeth * TW_TOOTH_ALPHA, dt);
float seconds = dt/refClock;
float acc_raw = (rpm_now - edgeBuf[1].rpm) / (seconds);*/
/*if(acc_raw * accel < 0){
accel = acc_raw;
}*/
float calc_rpm = 0;
/*if(!current_inj_rpm_available || current_inj_rpm_available < teeth_i){
for(int i = 1; i < TW_PERCYL_TEETH; i++){
if(!edgeBuf[i].isInj){
continue;
}
if(edgeBuf[i].rpm < 20 || !edgeBuf[i].new){
break;
}
current_inj_rpm_available = i;
calc_rpm = edgeBuf[i].rpm;
}
if(!current_inj_rpm_available){
float rpm0 = last_INJECTING_RPM; // your base RPM
float accel = last_accel; // RPM/s from your filter
// A) Adjust a time computed from an angle (e.g., 31° to EOI)
float dt_to_eoi = TimeToAngleDeg(MT_RPM, inj_accel, C_Angle); //tendria q ser mt rpm no las last injecting rpm.
// con last inj rpm, que es mas lento, da dt mayor, y tambien unas rpm menor y dt todavia mayor...
// B) Predict the RPM at that firing time, if you need it
float rpm_at_eoi = PredictRPM(rpm0, inj_accel, dt_to_eoi);
calc_rpm = last_INJECTING_RPM;
}
}else{
calc_rpm = edgeBuf[current_inj_rpm_available].rpm;
}*/
calc_rpm = TEETH_RPM;
int_time += Remaining_Angle / (calc_rpm*USTODEG);
//int_time += Remaining_Angle / (edgeBuf[idx].rpm*USTODEG);
for(int i = teeth_i; i < idx; i++){
if(edgeBuf[i].rpm < 20){
break;
}
if(edgeBuf[i].isInj){
}
int_time += TW_TOOTH_ALPHA / (calc_rpm*USTODEG);
//int_time += TW_TOOTH_ALPHA / (edgeBuf[i].rpm*USTODEG);
}
return int_time;
}
extern float target_eoi;
extern float real_eoi;
float TM_INTEGRATE_TIME_TO_EOI(float C_Angle, uint8_t teeth_i){
float int_time = 0.0;
uint8_t idx = C_Angle/TW_TOOTH_ALPHA; //empiezo diente despues de phi1
if(idx < teeth_i){
return 0.0f;
}
float Remaining_Angle = C_Angle - idx*TW_TOOTH_ALPHA;
/*uint8_t id = 0;
for (int i = 1; i < TW_PERCYL_TEETH; i++) {
if (edgeBuf[i].rpm < 20.0f) break;
if (!edgeBuf[i].isInj) continue;
if(!edgeBuf[i+1].isInj){
id = i;
}
}
if(!id){id = teeth_i;}
uint32_t t_diff_fast = ticks_diff(edgeBuf[2].ic, edgeBuf[1].ic);
uint32_t t_diff_slow = ticks_diff(edgeBuf[id].ic, edgeBuf[id-1].ic);*/
uint32_t t_diff_fast = ticks_diff(edgeBuf[idx+2].ic, edgeBuf[idx+1].ic);
int_time = Remaining_Angle/TW_TOOTH_ALPHA * ticks_diff(edgeBuf[idx+1].ic, edgeBuf[idx].ic)/16;
//int_time = Remaining_Angle/TW_TOOTH_ALPHA * t_diff_fast/16;
for(int i = teeth_i; i < idx; i++){
int_time += ticks_diff(edgeBuf[i+1].ic, edgeBuf[i].ic)/16;
//int_time += t_diff_fast/16;
}
/*float dt_to_eoi = TimeToAngleDeg(MT_RPM, last_accel, C_Angle); //tendria q ser mt rpm no las last injecting rpm.
// B) Predict the RPM at that firing time, if you need it
float rpm_next = PredictRPM(rpm0, last_accel, dt_to_eoi);
int_time *= rpm_next / MT_RPM;*/
return int_time;
}
float lpf_rpm = 1;
void TM_UPDATE_INJECTION_RPM_AND_ALPHA(float EOI_angle, float BIP_angle, uint32_t IC_EOI, uint32_t IC_BIP){
uint8_t idx2 = BIP_angle / TW_TOOTH_ALPHA + 1; // target tooth index
float eq_rpm = 0.0f;
uint8_t count = 0;
for (int i = idx2; i < TW_PERCYL_TEETH; i++) {
if (edgeBuf[i].rpm < 20.0f) break;
if (!edgeBuf[i].isInj) continue;
eq_rpm += edgeBuf[i].rpm;
count++;
if (!edgeBuf[i+1].isInj) {
eq_rpm += edgeBuf[i+1].rpm;
count++;
}
}
if (count) {
eq_rpm /= count; // averaged slowed rpm
} else {
eq_rpm = edgeBuf[idx2].rpm;
}
//last_INJECTING_RPM = eq_rpm;
uint8_t idx = TM_GET_IN_BUFFER_ID(IC_BIP);
uint32_t inj_Difference = IC_EOI >= IC_BIP ? IC_EOI-IC_BIP : IC_EOI-(IC_BIP-0xffffffff);
float alpha = fclamp(EOI_angle - BIP_angle, 0, 90);
eq_rpm = fclamp(60.0 * refClock/inj_Difference * alpha / 360, MIN_RPM, 4000);
//Otra forma de calcular las rpm
//seria calcular donde esta bip real y eoi real
//(en angulos, que ya lo tenemos, sacar el periodo en angulo y dividir entre el periodo en tiempo...
//por ahora calculamos rpm promedio y ya
uint8_t idx_eoi = TM_GET_IN_BUFFER_ID(IC_EOI);
last_INJECTING_RPM = edgeBuf[idx_eoi].rpm;
//last_INJECTING_RPM = alpha < 0.2 ? edgeBuf[idx-1].rpm : eq_rpm; voy a usarla para la aceleracion
//last_INJECTING_RPM += lpf_rpm*(edgeBuf[idx_eoi].rpm - last_INJECTING_RPM);
/*if(last_INJECTING_RPM < 50){
last_INJECTING_RPM++;
}*/
last_INJECTING_ALPHA = alpha;
last_non_Inj_RPM = edgeBuf[idx-2].rpm;
current_inj_rpm_available = 0;
//MT_Push(edgeBuf[0].rpm, TW_THEETHS * TW_TOOTH_ALPHA / CYLINDERS);
//uint32_t acc_Difference = edgeBuf[idx-1].ic >= edgeBuf[0].ic ? edgeBuf[idx-1].ic-edgeBuf[0].ic : edgeBuf[idx-1].ic-(edgeBuf[0].ic-0xffffffff);
}
float rpm_error_accel = 0.0;
float TM_UPDATE_ACCELERATION(float last_MT_RPM, float now_MT_RPM, uint32_t dt_mt){
//esto se da con la MT_RPM en su periodo, si usamos el siguiente enviemos last_rpm (despues de actualizar)
// A) Adjust a time computed from an angle (e.g., 31° to EOI)
float dt_seconds = dt_mt / refClock;
// con last inj rpm, que es mas lento, da dt mayor, y tambien unas rpm menor y dt todavia mayor...
// B) Predict the RPM at that firing time, if you need it
float theory_new_rpm = PredictRPM(last_MT_RPM, last_accel, dt_seconds);
rpm_error_accel = theory_new_rpm - now_MT_RPM;
if(rpm_error_accel > 50 || rpm_error_accel < -50){
MT_AccelReset();
//last_accel = MT_AccelUpdate(now_MT_RPM); //la aceleracion
}
last_accel = MT_AccelUpdate(now_MT_RPM); //la aceleracion
//last_accel = (now_MT_RPM - last_MT_RPM)/dt_seconds; //la aceleracion
float next_RPM = last_accel * dt_seconds + now_MT_RPM;
return next_RPM;
}
float eoi_acc_K = 0.18; //-0.2 - 0.08 0.18, last rover 0.25
float eoi_acc_K_1 = 0; //-0.2 - 0.08 0.18, last rover 0.25
float eoi_acc_K_2 = 0; //-0.2 - 0.08 0.18, last rover 0.25
float lpf_eoi_acc_k = 1; //probar,
float correction_eoi_accel = 0.0f;
float limit_1 = -1.5;
float limit_2 = 1; //en otro setting 0 mehor
float TM_GET_ACCEL_CORRECTION(float last_MT_RPM, float now_MT_RPM, float TEETH_RPM, uint8_t triggerTeeth){
//esto se da con la MT_RPM en su periodo, si usamos el siguiente enviemos last_rpm (despues de actualizar)
if(now_MT_RPM < 1){
correction_eoi_accel = 0;
return 0.0;
}
float rp_sum = now_MT_RPM + last_MT_RPM;
float dt = (1.0f / CYLINDERS) * (120.0f / rp_sum);
inj_accel = (edgeBuf[triggerTeeth].rpm - last_non_Inj_RPM)/dt;
////inj_accel = (now_MT_RPM - last_MT_RPM)/dt;
//inj_accel = (TEETH_RPM - last_INJECTING_RPM)/dt;
//inj_accel = (TEETH_RPM - edgeBuf[currentTooth].rpm)/dt;
//inj_accel = last_accel;
float comp = fclamp(-inj_accel * eoi_acc_K / now_MT_RPM, limit_1, limit_2); //-10, 10
/*if(inj_accel>0){
comp = inj_accel * eoi_acc_K_1/ now_MT_RPM;
}else{
comp = inj_accel * eoi_acc_K_2/ now_MT_RPM;
}*/
//last accel
correction_eoi_accel += lpf_eoi_acc_k*(comp-correction_eoi_accel);
//correction_eoi_accel = fclamp(comp, 0, -2000);
correction_eoi_accel = comp;
return correction_eoi_accel;
}
float last_to_inj_degs= 0;
float now_to_inj_degs = 0;
float newaccel = 0;
float TM_GET_INJ_ACCEL(uint8_t currentTooth){
uint16_t alpha = currentTooth * TW_TOOTH_ALPHA;
uint32_t ticks = ticks_diff(IC_RPM_Val2, edgeBuf[0].ic);
float dt = ticks / refClock;
last_to_inj_degs = now_to_inj_degs;
now_to_inj_degs = alpha / dt;
newaccel = (now_to_inj_degs - last_to_inj_degs)/dt;
float comp = fclamp(-newaccel * eoi_acc_K / MT_RPM, limit_1, limit_2); //-10, 10
correction_eoi_accel += lpf_eoi_acc_k*(comp-correction_eoi_accel);
return correction_eoi_accel;
}
//float bip_acc_K = 0.055;
float lpf_bip_acc_k = 1;
float correction_bip_accel = 0.0f;
float last_teeth_rpm_bip = 0.0;
void TM_PREPARE_ACCEL_CORRECTION_BIP(uint8_t triggerteeth){ //should be triggerteeth-1, and then evaluate comp 1 teeth before triggerteeth.
uint8_t idx = triggerteeth - 1;
last_teeth_rpm_bip = (idx && idx < TW_PERCYL_TEETH) ? edgeBuf[idx].rpm : -1;
}
float TM_UPDATE_ACCEL_CORRECTION_BIP(float last_MT_RPM, float now_MT_RPM, float TEETH_RPM){
float rp_sum = now_MT_RPM + last_MT_RPM;
float dt = (1.0f / CYLINDERS) * (120.0f / rp_sum);
float bip_accel = (TEETH_RPM - last_non_Inj_RPM)/dt;
float comp = fclamp(-bip_accel * bip_acc_K / now_MT_RPM, -10, 10);
correction_bip_accel += lpf_bip_acc_k*(comp-correction_bip_accel);
}
float TM_GET_ACCEL_CORRECTION_BIP(){
return correction_bip_accel;
}
uint16_t T_delayEnd = 20;
//float lpf_end_delay = 0.2;
void TM_UPDATE_END_DELAY_CORRECTION(){
uint32_t t = TM_TIME_FROM_NEAREST_TOOTH(IC_EOI, 1);
if(t - (T_integrated - T_delayEnd) < 100){
//T_delayEnd = t - (T_integrated - T_delayEnd);
//we were using this but bugging on borders
//T_delayEnd -= lpf_end_delay*(T_delayEnd - (t - T_integrated));
}else{
T_delayEnd = 20;
}
}
uint16_t TM_GET_T_DELAY_END(){
return T_delayEnd;
}
float TM_GET_LOCAL_ACC_TO_ANGLE(float C_Angle, uint8_t roundingMode){
uint8_t idx = C_Angle/TW_TOOTH_ALPHA; //empiezo diente despues de phi1
if(roundingMode > 1){
idx++;
}else if(roundingMode){
idx = (C_Angle+1.5f)/TW_TOOTH_ALPHA;
}
uint32_t accel_Difference = edgeBuf[idx].ic >= edgeBuf[0].ic ? edgeBuf[idx].ic-edgeBuf[0].ic : edgeBuf[idx].ic-(edgeBuf[0].ic-0xffffffff);
float now_rpm = idx * 60.0 * refClock/accel_Difference / TW_THEETHS;
return (now_rpm - edgeBuf[0].rpm)/idx;
}
/*
// ---- Tunables ----
#define SECTOR_DEG 90.0f // angle between your MT RPM samples (90° in your case)
#define EMA_ALPHA 0.010f // accel smoothing [0..1]0.015
#define RPM_MIN 50.0f // ignore junk below this
#define DT_MIN_S (SECTOR_DEG/360.0f * (120.0f / (2.0f*10000.0f))) // ~safety floor
// ---- State ----
static float s_prev_rpm = -1.0f; // negative => not seeded
static float s_acc_ema = 0.0f; // filtered accel (RPM/s)
// Call once per new missing-tooth RPM; returns latest accel in RPM/s
static inline float MT_AccelUpdate(float rpm_now)
{
if (rpm_now < RPM_MIN) return s_acc_ema; // ignore bogus
if (s_prev_rpm < 0.0f) { s_prev_rpm = rpm_now; return s_acc_ema; } // seed
// Δt from sector angle and average RPM between samples:
float rp_sum = rpm_now + s_prev_rpm;
if (rp_sum <= 1e-3f) rp_sum = 1e-3f;
float dt = (SECTOR_DEG / 360.0f) * (120.0f / rp_sum);
if (dt < DT_MIN_S) dt = DT_MIN_S;
// raw acceleration and simple clamp for sanity
float acc_raw = (rpm_now - s_prev_rpm) / dt;
if (acc_raw > 8000.0f) acc_raw = 8000.0f;
if (acc_raw < -8000.0f) acc_raw = -8000.0f;
// smooth it
s_acc_ema += EMA_ALPHA * (acc_raw - s_acc_ema);
s_prev_rpm = rpm_now;
return s_acc_ema;
}*/
// ---- Tunables ----
#define SECTOR_DEG 90.0f // angle between MT samples
float TAU_S = 0.1f; // smoothing time-constant (seconds) -> adjust to taste
#define RPM_MIN 50.0f
#define ACC_ABS_LIM 8000.0f
// ---- State ----
static float s_prev_rpm = -1.0f;
static float s_acc_ema = 0.0f;
// Call once per new missing-tooth RPM; returns filtered accel in RPM/s
static inline float MT_AccelUpdate(float rpm_now)
{
if (rpm_now < RPM_MIN) return s_acc_ema;
if (s_prev_rpm < 0.0f) { s_prev_rpm = rpm_now; return s_acc_ema; }
// Δt from sector angle and avg RPM between samples
float rp_sum = rpm_now + s_prev_rpm;
if (rp_sum <= 1e-3f) rp_sum = 1e-3f;
float dt = (SECTOR_DEG / 360.0f) * (120.0f / rp_sum); // seconds
// Raw accel (RPM/s)
float acc_raw = (rpm_now - s_prev_rpm) / dt;
if (acc_raw > ACC_ABS_LIM) acc_raw = ACC_ABS_LIM;
if (acc_raw < -ACC_ABS_LIM) acc_raw = -ACC_ABS_LIM;
// ---- Time-constant EMA ----
// Exact-discrete 1st-order low-pass: alpha = dt / (tau + dt)
float alpha = dt / (TAU_S + dt);
// Optional: keep alpha within sane bounds (helps extreme RPMs)
if (alpha < 0.01f) alpha = 0.01f; // very high RPM -> don't freeze
if (alpha > 0.6f) alpha = 0.6f; // very low RPM -> don't overshoot
s_acc_ema += alpha * (acc_raw - s_acc_ema);
s_prev_rpm = rpm_now;
return s_acc_ema;
}
static inline void MT_AccelReset()
{
s_acc_ema = 0;
}
// last_injecting_rpm: RPM0 (current)
// last_accel: a (RPM/s)
/* 1) Predict RPM after dt seconds */
static inline float PredictRPM(float rpm0, float accel_rpms, float dt_s)
{
return rpm0 + accel_rpms * dt_s;
}
/* 2) Time to sweep dtheta_deg with constant accel.
Returns seconds. Falls back to constant-speed if accel ~ 0. */
static inline float TimeToAngleDeg(float rpm0, float accel_rpms, float dtheta_deg)
{
const float eps = 1e-6f;
float w0 = rpm0 * 6.0f; // deg/s
float a = accel_rpms * 6.0f; // deg/s^2
if (dtheta_deg <= 0.0f || w0 <= eps) return 0.0f;
if (fabsf(a) < 1e-4f) {
// ~constant speed
return dtheta_deg / w0;
}
// Solve 0.5*a*t^2 + w0*t - dtheta = 0
float disc = w0*w0 + 2.0f*a*dtheta_deg;
if (disc <= 0.0f) {
// acceleration too negative to reach angle; fallback to constant-speed
return dtheta_deg / fmaxf(w0, eps);
}
float t = (-w0 + sqrtf(disc)) / a; // physical root for forward motion
if (t < 0.0f) {
// numerical/edge-case guard
return dtheta_deg / fmaxf(w0, eps);
}
return t;
}
Reference in New Issue View Git Blame Copy Permalink