1060 lines
32 KiB
C
1060 lines
32 KiB
C
/*
|
|
* 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;
|
|
}
|