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