static ALWAYS_INLINE void handleFuelInjectionEvent(int eventIndex, bool limitedFuel, InjectionEvent *event,
int rpm DECLARE_ENGINE_PARAMETER_S) {
if (limitedFuel)
return; // todo: move this check up
/**
* todo: this is a bit tricky with batched injection. is it? Does the same
* wetting coefficient works the same way for any injection mode, or is something
* x2 or /2?
*/
floatms_t injectionDuration = ENGINE(wallFuel).adjust(event->injectorIndex, ENGINE(fuelMs) PASS_ENGINE_PARAMETER);
ENGINE(actualLastInjection) = injectionDuration;
if (cisnan(injectionDuration)) {
warning(OBD_PCM_Processor_Fault, "NaN injection pulse");
return;
}
if (injectionDuration < 0) {
warning(OBD_PCM_Processor_Fault, "Negative injection pulse %f", injectionDuration);
return;
}
if (engine->isCylinderCleanupMode)
return;
floatus_t injectionStartDelayUs = ENGINE(rpmCalculator.oneDegreeUs) * event->injectionStart.angleOffset;
OutputSignal *signal = &ENGINE(engineConfiguration2)->fuelActuators[eventIndex];
if (event->isSimultanious) {
if (injectionDuration < 0) {
firmwareError("duration cannot be negative: %d", injectionDuration);
return;
}
if (cisnan(injectionDuration)) {
firmwareError("NaN in scheduleOutput", injectionDuration);
return;
}
/**
* this is pretty much copy-paste of 'scheduleOutput'
* 'scheduleOutput' is currently only used for injection, so maybe it should be
* changed into 'scheduleInjection' and unified? todo: think about it.
*/
efiAssertVoid(signal!=NULL, "signal is NULL");
int index = getRevolutionCounter() % 2;
scheduling_s * sUp = &signal->signalTimerUp[index];
scheduling_s * sDown = &signal->signalTimerDown[index];
scheduleTask("out up", sUp, (int) injectionStartDelayUs, (schfunc_t) &startSimultaniousInjection, engine);
scheduleTask("out down", sDown, (int) injectionStartDelayUs + MS2US(injectionDuration),
(schfunc_t) &endSimultaniousInjection, engine);
} else {
#if EFI_UNIT_TEST || defined(__DOXYGEN__)
printf("scheduling injection angle=%f/delay=%f injectionDuration=%f\r\n", event->injectionStart.angleOffset, injectionStartDelayUs, injectionDuration);
#endif
scheduleOutput(signal, getTimeNowUs(), injectionStartDelayUs, MS2US(injectionDuration), event->output);
}
}
/**
* @return per cylinder injection time, in Milliseconds
*/
floatms_t getSpeedDensityFuel(float map DECLARE_GLOBAL_SUFFIX) {
/**
* most of the values are pre-calculated for performance reasons
*/
float tChargeK = ENGINE(engineState.tChargeK);
if (cisnan(tChargeK)) {
warning(CUSTOM_ERR_TCHARGE_NOT_READY2, "tChargeK not ready"); // this would happen before we have CLT reading for example
return 0;
}
efiAssert(CUSTOM_ERR_ASSERT, !cisnan(map), "NaN map", 0);
float adjustedMap = map + engine->engineLoadAccelEnrichment.getEngineLoadEnrichment(PASS_GLOBAL_SIGNATURE);
efiAssert(CUSTOM_ERR_ASSERT, !cisnan(adjustedMap), "NaN adjustedMap", 0);
float airMass = getCylinderAirMass(ENGINE(engineState.currentBaroCorrectedVE), adjustedMap, tChargeK PASS_GLOBAL_SUFFIX);
if (cisnan(airMass)) {
warning(CUSTOM_ERR_6685, "NaN airMass");
return 0;
}
#if EFI_PRINTF_FUEL_DETAILS
printf("map=%.2f adjustedMap=%.2f airMass=%.2f\t\n",
map, adjustedMap, engine->engineState.airMass);
#endif /*EFI_PRINTF_FUEL_DETAILS */
engine->engineState.airMass = airMass;
return sdMath(airMass, ENGINE(engineState.targetAFR) PASS_GLOBAL_SUFFIX) * 1000;
}
void assertEqualsM(const char *msg, float expected, float actual) {
if (cisnan(actual) && !cisnan(expected)) {
printf("Unexpected: %s %.4f while expected %.4f\r\n", msg, actual, expected);
exit(-1);
}
float delta = absF(actual - expected);
if (delta > 0.0001) {
printf("delta: %.7f\r\n", delta);
printf("Unexpected: %s %.4f while expected %.4f\r\n", msg, actual, expected);
exit(-1);
}
printf("Validated %s: %f\r\n", msg, expected);
}
/**
* @return Fuel injection duration injection as specified in the fuel map, in milliseconds
*/
floatms_t getBaseTableFuel(engine_configuration_s *engineConfiguration, int rpm, float engineLoad) {
if (cisnan(engineLoad)) {
warning(OBD_PCM_Processor_Fault, "NaN engine load");
return NAN;
}
return fuelMap.getValue(engineLoad, rpm);
}
floatms_t WallFuel::adjust(int injectorIndex, floatms_t target DECLARE_ENGINE_PARAMETER_SUFFIX) {
if (cisnan(target)) {
return target;
}
// disable this correction for cranking
if (ENGINE(rpmCalculator).isCranking(PASS_ENGINE_PARAMETER_SIGNATURE)) {
return target;
}
float addedToWallCoef = CONFIG(addedToWallCoef);
/**
* What amount of fuel is sucked of the walls, based on current amount of fuel on the wall.
*/
floatms_t suckedOffWallsAmount = wallFuel[injectorIndex] * CONFIG(suckedOffCoef);
floatms_t adjustedFuelPulse = (target - suckedOffWallsAmount) / (1 - addedToWallCoef);
// We can't inject a negative amount of fuel
// If this goes below zero we will be over-fueling slightly,
// but that's ok.
if(adjustedFuelPulse < 0) {
adjustedFuelPulse = 0;
}
float addedToWallsAmount = adjustedFuelPulse * addedToWallCoef;
wallFuel[injectorIndex] += addedToWallsAmount - suckedOffWallsAmount;
engine->wallFuelCorrection = adjustedFuelPulse - target;
return adjustedFuelPulse;
}
/**
* This function adds an error if MAP sensor value is outside of expected range
* @return unchanged mapKPa parameter
*/
float validateMap(float mapKPa DECLARE_ENGINE_PARAMETER_S) {
if (cisnan(mapKPa) || mapKPa < CONFIG(mapErrorLowValue) || mapKPa > CONFIG(mapErrorHighValue)) {
warning(OBD_PCM_Processor_Fault, "invalid MAP value: %f", mapKPa);
return 0;
}
return mapKPa;
}
/** @brief Binary search
* @returns the highest index within sorted array such that array[i] is greater than or equal to the parameter
* @note If the parameter is smaller than the first element of the array, -1 is returned.
*/
int findIndex(float array[], int size, float value) {
if (cisnan(value))
fatal("NaN in findIndex\r\n");
if (value < array[0])
return -1;
int middle;
int left = 0;
int right = size;
while (1) {
if (size-- == 0)
fatal("Unexpected state in binary search.");
middle = (left + right) / 2;
// print("left=%d middle=%d right=%d: %f\r\n", left, middle, right, array[middle]);
if (middle == left)
break;
if (value < array[middle]) {
right = middle;
} else if (value > array[middle]) {
left = middle;
} else {
break;
}
}
return middle;
}
void updateAuxValves(DECLARE_ENGINE_PARAMETER_SIGNATURE) {
if (engineConfiguration->auxValves[0] == GPIO_UNASSIGNED) {
return;
}
float x = getTPS(PASS_ENGINE_PARAMETER_SIGNATURE);
if (cisnan(x)) {
// error should be already reported by now
return;
}
engine->engineState.auxValveStart = interpolate2d("aux", x,
engineConfiguration->fsioCurve1Bins,
engineConfiguration->fsioCurve1, FSIO_CURVE_16);
engine->engineState.auxValveEnd = interpolate2d("aux", x,
engineConfiguration->fsioCurve2Bins,
engineConfiguration->fsioCurve2, FSIO_CURVE_16);
if (engine->engineState.auxValveStart >= engine->engineState.auxValveEnd) {
// this is a fatal error to make this really visible
firmwareError(CUSTOM_AUX_OUT_OF_ORDER, "out of order at %.2f %.2f %.2f", x,
engine->engineState.auxValveStart,
engine->engineState.auxValveEnd);
}
}
void assertEqualsM3(const char *prefix, const char *message, float expected, float actual, float EPS) {
char msg[100];
strcpy(msg, prefix);
strcat(msg, message);
if (cisnan(actual) && !cisnan(expected)) {
printf("Assert failed: %s %.4f while expected %.4f\r\n", msg, actual, expected);
exit(-1);
}
float delta = absF(actual - expected);
if (delta > EPS) {
printf("delta: %.7f\r\n", delta);
printf("Unexpected: %s %.4f while expected %.4f\r\n", msg, actual, expected);
exit(-1);
}
printf("Validated %s: %f\r\n", msg, expected);
}
// http://rusefi.com/math/t_charge.html
float getTCharge(int rpm, float tps, float coolantTemp, float airTemp DECLARE_ENGINE_PARAMETER_SUFFIX) {
if (cisnan(coolantTemp) || cisnan(airTemp)) {
warning(CUSTOM_ERR_NAN_TCHARGE, "t-getTCharge NaN");
return coolantTemp;
}
float Tcharge_coff;
if (CONFIG(tChargeMode) == TCHARGE_MODE_AIR_INTERP) {
const floatms_t gramsPerMsToKgPerHour = (3600.0f * 1000.0f) / 1000.0f;
// We're actually using an 'old' airMass calculated for the previous cycle, but it's ok, we're not having any self-excitaton issues
floatms_t airMassForEngine = engine->engineState.airMass * CONFIG(specs.cylindersCount);
// airMass is in grams per 1 cycle for 1 cyl. Convert it to airFlow in kg/h for the engine.
// And if the engine is stopped (0 rpm), then airFlow is also zero (avoiding NaN division)
floatms_t airFlow = (rpm == 0) ? 0 : airMassForEngine * gramsPerMsToKgPerHour / getEngineCycleDuration(rpm PASS_ENGINE_PARAMETER_SUFFIX);
// just interpolate between user-specified min and max coefs, based on the max airFlow value
Tcharge_coff = interpolateClamped(0.0, CONFIG(tChargeAirCoefMin), CONFIG(tChargeAirFlowMax), CONFIG(tChargeAirCoefMax), airFlow);
// save it for console output (instead of MAF massAirFlow)
engine->engineState.airFlow = airFlow;
} else {
// TCHARGE_MODE_RPM_TPS
float minRpmKcurrentTPS = interpolateMsg("minRpm", tpMin, CONFIG(tChargeMinRpmMinTps), tpMax,
CONFIG(tChargeMinRpmMaxTps), tps);
float maxRpmKcurrentTPS = interpolateMsg("maxRpm", tpMin, CONFIG(tChargeMaxRpmMinTps), tpMax,
CONFIG(tChargeMaxRpmMaxTps), tps);
Tcharge_coff = interpolateMsg("Kcurr", rpmMin, minRpmKcurrentTPS, rpmMax, maxRpmKcurrentTPS, rpm);
}
if (cisnan(Tcharge_coff)) {
warning(CUSTOM_ERR_T2_CHARGE, "t2-getTCharge NaN");
return coolantTemp;
}
// We use a robust interp. function for proper tcharge_coff clamping.
float Tcharge = interpolateClamped(0.0f, coolantTemp, 1.0f, airTemp, Tcharge_coff);
if (cisnan(Tcharge)) {
// we can probably end up here while resetting engine state - interpolation would fail
warning(CUSTOM_ERR_TCHARGE_NOT_READY, "getTCharge NaN");
return coolantTemp;
}
return Tcharge;
}
/**
* @brief Injector lag correction
* @param vBatt Battery voltage.
* @return Time in ms for injection opening time based on current battery voltage
*/
floatms_t getInjectorLag(float vBatt DECLARE_ENGINE_PARAMETER_S) {
if (cisnan(vBatt)) {
warning(OBD_System_Voltage_Malfunction, "vBatt=%f", vBatt);
return engineConfiguration->injector.lag;
}
float vBattCorrection = interpolate2d(vBatt, engineConfiguration->injector.battLagCorrBins,
engineConfiguration->injector.battLagCorr, VBAT_INJECTOR_CURVE_SIZE);
return engineConfiguration->injector.lag + vBattCorrection;
}
void AccelEnrichmemnt::onNewValue(float currentValue DECLARE_ENGINE_PARAMETER_S) {
if (!cisnan(this->currentValue)) {
delta = currentValue - this->currentValue;
FuelSchedule *fs = &engine->engineConfiguration2->injectionEvents;
cb.add(delta * fs->eventsCount);
}
this->currentValue = currentValue;
}
/**
* @return value in seconds
*/
float sdMath(engine_configuration_s *engineConfiguration, float VE, float MAP, float AFR, float temp) {
if (MAP < 0.001 || cisnan(MAP)) {
warning(OBD_PCM_Processor_Fault, "invalid MAP value");
return 0;
}
float injectorFlowRate = cc_minute_to_gramm_second(engineConfiguration->injectorFlow);
float Vol = engineConfiguration->displacement / engineConfiguration->cylindersCount;
return (Vol * VE * MAP) / (AFR * injectorFlowRate * GAS_R * temp);
}
/**
* @return Spark dwell time, in milliseconds.
*/
float getSparkDwellMsT(engine_configuration_s *engineConfiguration, int rpm) {
if (isCrankingR(rpm)) {
// technically this could be implemented via interpolate2d
float angle = engineConfiguration->crankingChargeAngle;
return getOneDegreeTimeMs(rpm) * angle;
}
efiAssert(!cisnan(rpm), "invalid rpm", NAN);
return interpolate2d(rpm, engineConfiguration->sparkDwellBins, engineConfiguration->sparkDwell, DWELL_CURVE_SIZE);
}
void PwmConfig::setFrequency(float frequency) {
if (cisnan(frequency)) {
// explicit code just to be sure
periodNt = NAN;
return;
}
/**
* see #handleCycleStart()
*/
periodNt = US2NT(frequency2periodUs(frequency));
}
void AccelEnrichmemnt::onNewValue(float currentValue DECLARE_ENGINE_PARAMETER_S) {
if (!cisnan(previousValue)) {
/**
* this could be negative, zero or positive
*/
float delta = currentValue - previousValue;
FuelSchedule *fs = engine->engineConfiguration2->injectionEvents;
cb.add(delta * fs->eventsCount);
}
previousValue = currentValue;
}
floatms_t getCrankingFuel3(float coolantTemperature,
uint32_t revolutionCounterSinceStart DECLARE_ENGINE_PARAMETER_S) {
// these magic constants are in Celsius
float baseCrankingFuel = engineConfiguration->cranking.baseFuel;
if (cisnan(coolantTemperature))
return baseCrankingFuel;
float durationCoef = interpolate2d(revolutionCounterSinceStart, config->crankingCycleBins,
config->crankingCycleCoef, CRANKING_CURVE_SIZE);
return interpolate2d(coolantTemperature, config->crankingFuelBins,
config->crankingFuelCoef, CRANKING_CURVE_SIZE) * baseCrankingFuel * durationCoef;
}
/**
* set_etb X
* manual duty cycle control without PID. Percent value from 0 to 100
*/
void setThrottleDutyCycle(float level) {
scheduleMsg(&logger, "setting ETB duty=%f%%", level);
if (cisnan(level)) {
directPwmValue = NAN;
return;
}
float dc = PERCENT_TO_DUTY(level);
directPwmValue = dc;
etb1.dcMotor.Set(dc);
scheduleMsg(&logger, "duty ETB duty=%f", dc);
}
TEST(misc, testMisc) {
print("******************************************* testMisc\r\n");
strcpy(buff, " ab ");
// we need a mutable array here
ASSERT_TRUE(strEqual("ab", efiTrim(buff)));
{
float v = atoff("1.0");
assertEqualsM("atoff", 1.0, v);
}
{
float v = atoff("nan");
ASSERT_TRUE(cisnan(v)) << "NaN atoff";
}
{
float v = atoff("N");
ASSERT_TRUE(cisnan(v)) << "NaN atoff";
}
// ASSERT_EQ(true, strEqual("spa3", getPinName(SPARKOUT_3_OUTPUT)));
// ASSERT_EQ(SPARKOUT_12_OUTPUT, getPinByName("spa12"));
}
percent_t getTPS(DECLARE_ENGINE_PARAMETER_SIGNATURE) {
#if !EFI_PROD_CODE
if (!cisnan(engine->mockTpsValue)) {
return engine->mockTpsValue;
}
#endif /* EFI_PROD_CODE */
if (!hasTpsSensor(PASS_ENGINE_PARAMETER_SIGNATURE))
return NO_TPS_MAGIC_VALUE;
// todo: if (config->isDualTps)
// todo: blah blah
// todo: if two TPS do not match - show OBD code via malfunction_central.c
return getPrimatyRawTPS(PASS_ENGINE_PARAMETER_SIGNATURE);
}
/**
* Schedules a callback 'angle' degree of crankshaft from now.
* The callback would be executed once after the duration of time which
* it takes the crankshaft to rotate to the specified angle.
*/
void scheduleByAngle(int rpm, scheduling_s *timer, angle_t angle, schfunc_t callback, void *param) {
if (!isValidRpm(rpm)) {
/**
* this might happen in case of a single trigger event after a pause - this is normal, so no
* warning here
*/
return;
}
float delayUs = getOneDegreeTimeUs(rpm) * angle;
if (cisnan(delayUs)) {
firmwareError("NaN delay?");
return;
}
scheduleTask("by angle", timer, (int) delayUs, callback, param);
}
/**
* @return Next time for signal toggle
*/
efitimeus_t PwmConfig::togglePwmState() {
#if DEBUG_PWM
scheduleMsg(&logger, "togglePwmState phaseIndex=%d iteration=%d", safe.phaseIndex, safe.iteration);
scheduleMsg(&logger, "period=%.2f safe.period=%.2f", period, safe.period);
#endif
if (cisnan(periodNt)) {
/**
* NaN period means PWM is paused
*/
return getTimeNowUs() + MS2US(100);
}
handleCycleStart();
/**
* Here is where the 'business logic' - the actual pin state change is happening
*/
// callback state index is offset by one. todo: why? can we simplify this?
int cbStateIndex = safe.phaseIndex == 0 ? phaseCount - 1 : safe.phaseIndex - 1;
stateChangeCallback(this, cbStateIndex);
efitimeus_t nextSwitchTimeUs = getNextSwitchTimeUs(this);
#if DEBUG_PWM
scheduleMsg(&logger, "%s: nextSwitchTime %d", state->name, nextSwitchTime);
#endif /* DEBUG_PWM */
// signed value is needed here
// int64_t timeToSwitch = nextSwitchTimeUs - getTimeNowUs();
// if (timeToSwitch < 1) {
// /**
// * We are here if we are late for a state transition.
// * At 12000RPM=200Hz with a 60 toothed wheel we need to change state every
// * 1000000 / 200 / 120 = ~41 uS. We are kind of OK.
// *
// * We are also here after a flash write. Flash write freezes the whole chip for a couple of seconds,
// * so PWM generation and trigger simulation generation would have to recover from this time lag.
// */
// //todo: introduce error and test this error handling warning(OBD_PCM_Processor_Fault, "PWM: negative switch time");
// timeToSwitch = 10;
// }
safe.phaseIndex++;
if (safe.phaseIndex == phaseCount) {
safe.phaseIndex = 0; // restart
safe.iteration++;
}
return nextSwitchTimeUs;
}
/**
*
* @param delay the number of ticks before the output signal
* immediate output if delay is zero
* @param dwell the number of ticks of output duration
*
*/
void scheduleOutput(OutputSignal *signal, float delayMs, float durationMs) {
if (durationMs < 0) {
firmwareError("duration cannot be negative: %d", durationMs);
return;
}
if (cisnan(durationMs)) {
firmwareError("NaN in scheduleOutput", durationMs);
return;
}
int index = getRevolutionCounter() % 2;
scheduling_s * sUp = &signal->signalTimerUp[index];
scheduling_s * sDown = &signal->signalTimerDown[index];
scheduleTask("out up", sUp, (int)MS2US(delayMs), (schfunc_t) &turnPinHigh, (void *) signal->io_pin);
scheduleTask("out down", sDown, (int)MS2US(delayMs + durationMs), (schfunc_t) &turnPinLow, (void*) signal->io_pin);
}
static void setFloat(const char *offsetStr, const char *valueStr) {
int offset = atoi(offsetStr);
if (absI(offset) == absI(ERROR_CODE)) {
scheduleMsg(&logger, "invalid offset [%s]", offsetStr);
return;
}
if (isOutOfBounds(offset))
return;
float value = atoff(valueStr);
if (cisnan(value)) {
scheduleMsg(&logger, "invalid value [%s]", valueStr);
return;
}
float *ptr = (float *) (&((char *) engine->engineConfiguration)[offset]);
*ptr = value;
getFloat(offset);
}
floatms_t WallFuel::adjust(int injectorIndex, floatms_t target DECLARE_ENGINE_PARAMETER_S) {
if (cisnan(target)) {
return target;
}
float addedToWallCoef = CONFIG(addedToWallCoef);
/**
* What amount of fuel is sucked of the walls, based on current amount of fuel on the wall.
*/
floatms_t suckedOffWallsAmount = wallFuel[injectorIndex] * CONFIG(suckedOffCoef);
floatms_t adjustedFuelPulse = (target - suckedOffWallsAmount) / (1 - addedToWallCoef);
float addedToWallsAmount = adjustedFuelPulse * addedToWallCoef;
wallFuel[injectorIndex] += addedToWallsAmount - suckedOffWallsAmount;
engine->wallFuelCorrection = adjustedFuelPulse - target;
return adjustedFuelPulse;
}
/**
*
* @param delay the number of ticks before the output signal
* immediate output if delay is zero
* @param dwell the number of ticks of output duration
*
*/
void scheduleOutput(OutputSignal *signal, efitimeus_t nowUs, float delayUs, float durationUs) {
#if EFI_GPIO
if (durationUs < 0) {
warning(OBD_PCM_Processor_Fault, "duration cannot be negative: %d", durationUs);
return;
}
if (cisnan(durationUs)) {
warning(OBD_PCM_Processor_Fault, "NaN in scheduleOutput", durationUs);
return;
}
efiAssertVoid(signal!=NULL, "signal is NULL");
int index = getRevolutionCounter() % 2;
scheduling_s * sUp = &signal->signalTimerUp[index];
scheduling_s * sDown = &signal->signalTimerDown[index];
scheduleByTime("out up", sUp, nowUs + (int) delayUs, (schfunc_t) &turnPinHigh, signal->output);
scheduleByTime("out down", sDown, nowUs + (int) (delayUs + durationUs), (schfunc_t) &turnPinLow, signal->output);
#endif
}
/**
* @return per cylinder injection time, in seconds
*/
float sdMath(engine_configuration_s *engineConfiguration, float VE, float MAP, float AFR, float tempK) {
if (MAP < 0.001 || cisnan(MAP)) {
warning(OBD_PCM_Processor_Fault, "invalid MAP value");
return 0;
}
/**
* todo: pre-calculate gramm/second injector flow to save one multiplication
* open question if that's needed since that's just a multiplication
*/
float injectorFlowRate = cc_minute_to_gramm_second(engineConfiguration->injector.flow);
// todo: pre-calculate cylinder displacement to save one division
float cylinderDisplacement = engineConfiguration->specs.displacement / engineConfiguration->specs.cylindersCount;
float airMass = (cylinderDisplacement * VE * MAP) / (GAS_R * tempK);
/**
* injection_pulse_duration = fuel_mass / injector_flow
* fuel_mass = air_mass / target_afr
*
* injection_pulse_duration = (air_mass / target_afr) / injector_flow
*/
return airMass / (AFR * injectorFlowRate);
}
float getIatCorrection(float iat DECLARE_ENGINE_PARAMETER_S) {
if (cisnan(iat))
return 1; // this error should be already reported somewhere else, let's just handle it
return interpolate2d(iat, config->iatFuelCorrBins, config->iatFuelCorr, IAT_CURVE_SIZE);
}
void PeriodicTask(efitime_t nowNt) override {
UNUSED(nowNt);
setPeriod(GET_PERIOD_LIMITED(&engineConfiguration->etb));
// set debug_mode 17
if (engineConfiguration->debugMode == DBG_ELECTRONIC_THROTTLE_PID) {
#if EFI_TUNER_STUDIO
pid.postState(&tsOutputChannels);
tsOutputChannels.debugIntField5 = feedForward;
#endif /* EFI_TUNER_STUDIO */
} else if (engineConfiguration->debugMode == DBG_ELECTRONIC_THROTTLE_EXTRA) {
#if EFI_TUNER_STUDIO
// set debug_mode 29
tsOutputChannels.debugFloatField1 = directPwmValue;
#endif /* EFI_TUNER_STUDIO */
}
if (shouldResetPid) {
pid.reset();
shouldResetPid = false;
}
if (!cisnan(directPwmValue)) {
etb1.dcMotor.Set(directPwmValue);
return;
}
if (boardConfiguration->pauseEtbControl) {
etb1.dcMotor.Set(0);
return;
}
percent_t actualThrottlePosition = getTPS();
if (engine->etbAutoTune) {
autoTune.input = actualThrottlePosition;
bool result = autoTune.Runtime(&logger);
tuneWorkingPid.updateFactors(autoTune.output, 0, 0);
float value = tuneWorkingPid.getOutput(50, actualThrottlePosition);
scheduleMsg(&logger, "AT input=%f output=%f PID=%f", autoTune.input,
autoTune.output,
value);
scheduleMsg(&logger, "AT PID=%f", value);
etb1.dcMotor.Set(PERCENT_TO_DUTY(value));
if (result) {
scheduleMsg(&logger, "GREAT NEWS! %f/%f/%f", autoTune.GetKp(), autoTune.GetKi(), autoTune.GetKd());
}
return;
}
percent_t targetPosition = getPedalPosition(PASS_ENGINE_PARAMETER_SIGNATURE);
feedForward = interpolate2d("etbb", targetPosition, engineConfiguration->etbBiasBins, engineConfiguration->etbBiasValues, ETB_BIAS_CURVE_LENGTH);
pid.iTermMin = engineConfiguration->etb_iTermMin;
pid.iTermMax = engineConfiguration->etb_iTermMax;
currentEtbDuty = feedForward +
pid.getOutput(targetPosition, actualThrottlePosition);
etb1.dcMotor.Set(PERCENT_TO_DUTY(currentEtbDuty));
if (engineConfiguration->isVerboseETB) {
pid.showPidStatus(&logger, "ETB");
}
}
void handleActionWithParameter(TokenCallback *current, char *parameter) {
while (parameter[0] == SPACE_CHAR) {
parameter[0] = 0;
parameter++;
}
switch (current->parameterType) {
case STRING_PARAMETER:
{
VoidCharPtr callbackS = (VoidCharPtr) current->callback;
(*callbackS)(parameter);
return;
}
case STRING_PARAMETER_P:
{
VoidCharPtrVoidPtr callbackS = (VoidCharPtrVoidPtr) current->callback;
(*callbackS)(parameter, current->param);
return;
}
default:
// todo: handle all cases explicitly
break;
}
// todo: refactor this hell!
if (current->parameterType == STRING2_PARAMETER || current->parameterType == STRING2_PARAMETER_P) {
int spaceIndex = findEndOfToken(parameter);
if (spaceIndex == -1) {
return;
}
REPLACE_SPACES_WITH_ZERO;
char * param0 = parameter;
parameter += spaceIndex + 1;
char * param1 = parameter;
if (current->parameterType == STRING2_PARAMETER) {
VoidCharPtrCharPtr callbackS = (VoidCharPtrCharPtr) current->callback;
(*callbackS)(param0, param1);
} else {
VoidCharPtrCharPtrVoidPtr callbackS = (VoidCharPtrCharPtrVoidPtr) current->callback;
(*callbackS)(param0, param1, current->param);
}
return;
}
if (current->parameterType == STRING3_PARAMETER) {
int spaceIndex = findEndOfToken(parameter);
if (spaceIndex == -1) {
return;
}
REPLACE_SPACES_WITH_ZERO;
char * param0 = parameter;
parameter += spaceIndex + 1;
spaceIndex = findEndOfToken(parameter);
if (spaceIndex == -1)
return;
REPLACE_SPACES_WITH_ZERO;
char * param1 = parameter;
parameter += spaceIndex + 1;
char * param2 = parameter;
VoidCharPtrCharPtrCharPtr callbackS = (VoidCharPtrCharPtrCharPtr) current->callback;
(*callbackS)(param0, param1, param2);
return;
}
// todo: refactor this hell!
if (current->parameterType == STRING5_PARAMETER) {
int spaceIndex = findEndOfToken(parameter);
if (spaceIndex == -1) {
return;
}
REPLACE_SPACES_WITH_ZERO;
char * param0 = parameter;
parameter += spaceIndex + 1;
spaceIndex = findEndOfToken(parameter);
if (spaceIndex == -1)
return;
REPLACE_SPACES_WITH_ZERO;
char * param1 = parameter;
parameter += spaceIndex + 1;
spaceIndex = findEndOfToken(parameter);
if (spaceIndex == -1)
return;
REPLACE_SPACES_WITH_ZERO;
char * param2 = parameter;
parameter += spaceIndex + 1;
spaceIndex = findEndOfToken(parameter);
if (spaceIndex == -1)
return;
REPLACE_SPACES_WITH_ZERO;
char * param3 = parameter;
parameter += spaceIndex + 1;
char * param4 = parameter;
VoidCharPtrCharPtrCharPtrCharPtrCharPtr callbackS = (VoidCharPtrCharPtrCharPtrCharPtrCharPtr) current->callback;
(*callbackS)(param0, param1, param2, param3, param4);
return;
}
if (current->parameterType == TWO_INTS_PARAMETER) {
int spaceIndex = findEndOfToken(parameter);
if (spaceIndex == -1)
return;
REPLACE_SPACES_WITH_ZERO;
int value1 = atoi(parameter);
if (absI(value1) == ERROR_CODE) {
#if (EFI_PROD_CODE || EFI_SIMULATOR) || defined(__DOXYGEN__)
scheduleMsg(logging, "not an integer [%s]", parameter);
#endif
return;
}
parameter += spaceIndex + 1;
int value2 = atoi(parameter);
if (absI(value2) == ERROR_CODE) {
#if (EFI_PROD_CODE || EFI_SIMULATOR) || defined(__DOXYGEN__)
scheduleMsg(logging, "not an integer [%s]", parameter);
#endif
return;
}
VoidIntInt callbackS = (VoidIntInt) current->callback;
(*callbackS)(value1, value2);
return;
}
if (current->parameterType == FLOAT_PARAMETER) {
float value = atoff(parameter);
if (cisnan(value)) {
print("invalid float [%s]\r\n", parameter);
return;
}
VoidFloat callbackF = (VoidFloat) current->callback;
// invoke callback function by reference
(*callbackF)(value);
return;
}
if (current->parameterType == FLOAT_FLOAT_PARAMETER || current->parameterType == FLOAT_FLOAT_PARAMETER_P) {
int spaceIndex = findEndOfToken(parameter);
if (spaceIndex == -1)
return;
REPLACE_SPACES_WITH_ZERO;
float value1 = atoff(parameter);
if (cisnan(value1)) {
print("invalid float [%s]\r\n", parameter);
return;
}
parameter += spaceIndex + 1;
float value2 = atoff(parameter);
if (cisnan(value2)) {
print("invalid float [%s]\r\n", parameter);
return;
}
if (current->parameterType == FLOAT_FLOAT_PARAMETER) {
VoidFloatFloat callbackS = (VoidFloatFloat) current->callback;
(*callbackS)(value1, value2);
} else {
VoidFloatFloatVoidPtr callbackS = (VoidFloatFloatVoidPtr) current->callback;
(*callbackS)(value1, value2, current->param);
}
return;
}
int value = atoi(parameter);
if (absI(value) == ERROR_CODE) {
print("invalid integer [%s]\r\n", parameter);
return;
}
if (current->parameterType == ONE_PARAMETER_P) {
VoidIntVoidPtr callback1 = (VoidIntVoidPtr) current->callback;
// invoke callback function by reference
(*callback1)(value, current->param);
} else {
VoidInt callback1 = (VoidInt) current->callback;
// invoke callback function by reference
(*callback1)(value);
}
}
