From 52ee60036a55ec0e8722bd68c7f4bbd66e566a7d Mon Sep 17 00:00:00 2001 From: rusefi Date: Mon, 29 May 2017 11:31:07 -0400 Subject: [PATCH] better error handling --- firmware/controllers/algo/accel_enrichment.cpp | 2 +- firmware/controllers/algo/engine.cpp | 10 +++++----- firmware/controllers/algo/fuel_math.cpp | 14 +++++++------- firmware/controllers/core/interpolation.cpp | 6 +++++- firmware/controllers/core/interpolation.h | 2 +- firmware/controllers/idle_thread.cpp | 2 +- firmware/controllers/math/engine_math.cpp | 2 +- unit_tests/test_basic_math/test_find_index.cpp | 8 ++++---- 8 files changed, 25 insertions(+), 21 deletions(-) diff --git a/firmware/controllers/algo/accel_enrichment.cpp b/firmware/controllers/algo/accel_enrichment.cpp index 14a2b2a21f..565f90530d 100644 --- a/firmware/controllers/algo/accel_enrichment.cpp +++ b/firmware/controllers/algo/accel_enrichment.cpp @@ -146,7 +146,7 @@ float AccelEnrichmemnt::getEngineLoadEnrichment(DECLARE_ENGINE_PARAMETER_SIGNATU if (distance <= 0) // checking if indexes are out of order due to circular buffer nature distance += minI(cb.getCount(), cb.getSize()); - taper = interpolate2d(distance, engineConfiguration->mapAccelTaperBins, engineConfiguration->mapAccelTaperMult, MAP_ACCEL_TAPER); + taper = interpolate2d("accel", distance, engineConfiguration->mapAccelTaperBins, engineConfiguration->mapAccelTaperMult, MAP_ACCEL_TAPER); result = taper * d * engineConfiguration->engineLoadAccelEnrichmentMultiplier; } else if (d < -engineConfiguration->engineLoadDecelEnleanmentThreshold) { diff --git a/firmware/controllers/algo/engine.cpp b/firmware/controllers/algo/engine.cpp index 88a4ce2b67..eecee4d6f9 100644 --- a/firmware/controllers/algo/engine.cpp +++ b/firmware/controllers/algo/engine.cpp @@ -173,7 +173,7 @@ void EngineState::updateSlowSensors(DECLARE_ENGINE_PARAMETER_SIGNATURE) { engine->sensors.iat = getIntakeAirTemperature(PASS_ENGINE_PARAMETER_SIGNATURE); engine->sensors.clt = getCoolantTemperature(PASS_ENGINE_PARAMETER_SIGNATURE); - warmupTargetAfr = interpolate2d(engine->sensors.clt, engineConfiguration->warmupTargetAfrBins, + warmupTargetAfr = interpolate2d("warm", engine->sensors.clt, engineConfiguration->warmupTargetAfrBins, engineConfiguration->warmupTargetAfr, WARMUP_TARGET_AFR_SIZE); } @@ -214,7 +214,7 @@ void EngineState::periodicFastCallback(DECLARE_ENGINE_PARAMETER_SIGNATURE) { cltTimingCorrection = getCltTimingCorrection(PASS_ENGINE_PARAMETER_SIGNATURE); - engineNoiseHipLevel = interpolate2d(rpm, engineConfiguration->knockNoiseRpmBins, + engineNoiseHipLevel = interpolate2d("knock", rpm, engineConfiguration->knockNoiseRpmBins, engineConfiguration->knockNoise, ENGINE_NOISE_CURVE_SIZE); baroCorrection = getBaroCorrection(PASS_ENGINE_PARAMETER_SIGNATURE); @@ -254,7 +254,7 @@ void Engine::preCalculate() { */ for (int i = 0; i < MAF_DECODING_CACHE_SIZE; i++) { float volts = i / MAF_DECODING_CACHE_MULT; - float maf = interpolate2d(volts, config->mafDecodingBins, + float maf = interpolate2d("maf", volts, config->mafDecodingBins, config->mafDecoding, MAF_DECODING_COUNT); mafDecodingLookup[i] = maf; } @@ -355,7 +355,7 @@ void Engine::periodicFastCallback(DECLARE_ENGINE_PARAMETER_SIGNATURE) { if (isValidRpm(rpm)) { MAP_sensor_config_s * c = &engineConfiguration->map; - angle_t start = interpolate2d(rpm, c->samplingAngleBins, c->samplingAngle, MAP_ANGLE_SIZE); + angle_t start = interpolate2d("mapa", rpm, c->samplingAngleBins, c->samplingAngle, MAP_ANGLE_SIZE); angle_t offsetAngle = TRIGGER_SHAPE(eventAngles[CONFIG(mapAveragingSchedulingAtIndex)]); @@ -365,7 +365,7 @@ void Engine::periodicFastCallback(DECLARE_ENGINE_PARAMETER_SIGNATURE) { fixAngle(cylinderStart, "cylinderStart"); engine->engineState.mapAveragingStart[i] = cylinderStart; } - engine->engineState.mapAveragingDuration = interpolate2d(rpm, c->samplingWindowBins, c->samplingWindow, MAP_WINDOW_SIZE); + engine->engineState.mapAveragingDuration = interpolate2d("samp", rpm, c->samplingWindowBins, c->samplingWindow, MAP_WINDOW_SIZE); } else { for (int i = 0; i < engineConfiguration->specs.cylindersCount; i++) { engine->engineState.mapAveragingStart[i] = NAN; diff --git a/firmware/controllers/algo/fuel_math.cpp b/firmware/controllers/algo/fuel_math.cpp index 17df6a57d6..8e4ecf546a 100644 --- a/firmware/controllers/algo/fuel_math.cpp +++ b/firmware/controllers/algo/fuel_math.cpp @@ -184,7 +184,7 @@ floatms_t getInjectorLag(float vBatt DECLARE_ENGINE_PARAMETER_SUFFIX) { warning(OBD_System_Voltage_Malfunction, "vBatt=%f", vBatt); return 0; } - float vBattCorrection = interpolate2d(vBatt, engineConfiguration->injector.battLagCorrBins, + float vBattCorrection = interpolate2d("lag", vBatt, engineConfiguration->injector.battLagCorrBins, engineConfiguration->injector.battLagCorr, VBAT_INJECTOR_CURVE_SIZE); return vBattCorrection; } @@ -205,19 +205,19 @@ void prepareFuelMap(DECLARE_ENGINE_PARAMETER_SIGNATURE) { float getCltFuelCorrection(DECLARE_ENGINE_PARAMETER_SIGNATURE) { if (cisnan(engine->sensors.clt)) return 1; // this error should be already reported somewhere else, let's just handle it - return interpolate2d(engine->sensors.clt, config->cltFuelCorrBins, config->cltFuelCorr, CLT_CURVE_SIZE) / PERCENT_MULT; + return interpolate2d("cltf", engine->sensors.clt, config->cltFuelCorrBins, config->cltFuelCorr, CLT_CURVE_SIZE) / PERCENT_MULT; } angle_t getCltTimingCorrection(DECLARE_ENGINE_PARAMETER_SIGNATURE) { if (cisnan(engine->sensors.clt)) return 0; // this error should be already reported somewhere else, let's just handle it - return interpolate2d(engine->sensors.clt, engineConfiguration->cltTimingBins, engineConfiguration->cltTimingExtra, CLT_TIMING_CURVE_SIZE); + return interpolate2d("timc", engine->sensors.clt, engineConfiguration->cltTimingBins, engineConfiguration->cltTimingExtra, CLT_TIMING_CURVE_SIZE); } float getIatFuelCorrection(float iat DECLARE_ENGINE_PARAMETER_SUFFIX) { 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); + return interpolate2d("iatc", iat, config->iatFuelCorrBins, config->iatFuelCorr, IAT_CURVE_SIZE); } /** @@ -261,13 +261,13 @@ floatms_t getCrankingFuel3(float coolantTemperature, float baseCrankingFuel = engineConfiguration->cranking.baseFuel; if (cisnan(coolantTemperature)) // todo: move this check down, below duration correction? return baseCrankingFuel; - float durationCoef = interpolate2d(revolutionCounterSinceStart, config->crankingCycleBins, + float durationCoef = interpolate2d("crank", revolutionCounterSinceStart, config->crankingCycleBins, config->crankingCycleCoef, CRANKING_CURVE_SIZE); - float coolantTempCoef = interpolate2d(coolantTemperature, config->crankingFuelBins, + float coolantTempCoef = interpolate2d("crank", coolantTemperature, config->crankingFuelBins, config->crankingFuelCoef, CRANKING_CURVE_SIZE); - float tpsCoef = interpolate2d(getTPS(PASS_ENGINE_PARAMETER_SIGNATURE), engineConfiguration->crankingTpsBins, + float tpsCoef = interpolate2d("crank", getTPS(PASS_ENGINE_PARAMETER_SIGNATURE), engineConfiguration->crankingTpsBins, engineConfiguration->crankingTpsCoef, CRANKING_CURVE_SIZE); return baseCrankingFuel * durationCoef * coolantTempCoef * tpsCoef; diff --git a/firmware/controllers/core/interpolation.cpp b/firmware/controllers/core/interpolation.cpp index bbcc2252d3..6a52eb8ce2 100644 --- a/firmware/controllers/core/interpolation.cpp +++ b/firmware/controllers/core/interpolation.cpp @@ -210,7 +210,11 @@ int findIndex(const float array[], int size, float value) { /** * @brief One-dimensional table lookup with linear interpolation */ -float interpolate2d(float value, float bin[], float values[], int size) { +float interpolate2d(const char *msg, float value, float bin[], float values[], int size) { + if (isnan(value)) { + firmwareError(CUSTOM_ERR_6533, "NaN in interpolate2d %s", msg); + return NAN; + } int index = findIndexMsg("value", bin, size, value); if (index == -1) diff --git a/firmware/controllers/core/interpolation.h b/firmware/controllers/core/interpolation.h index ab470d7538..26f83d9ad1 100644 --- a/firmware/controllers/core/interpolation.h +++ b/firmware/controllers/core/interpolation.h @@ -25,7 +25,7 @@ int findIndexMsg(const char *msg, const float array[], int size, float value); int findIndex2(const float array[], unsigned size, float value); float interpolate(float x1, float y1, float x2, float y2, float x); float interpolateMsg(const char *msg, float x1, float y1, float x2, float y2, float x); -float interpolate2d(float value, float bin[], float values[], int size); +float interpolate2d(const char *msg, float value, float bin[], float values[], int size); int needInterpolationLogging(void); diff --git a/firmware/controllers/idle_thread.cpp b/firmware/controllers/idle_thread.cpp index 6e1de475ed..ddf0198eda 100644 --- a/firmware/controllers/idle_thread.cpp +++ b/firmware/controllers/idle_thread.cpp @@ -207,7 +207,7 @@ static msg_t ivThread(int param) { finishIdleTestIfNeeded(); undoIdleBlipIfNeeded(); - float cltCorrection = interpolate2d(engine->sensors.clt, config->cltIdleCorrBins, config->cltIdleCorr, + float cltCorrection = interpolate2d("cltT", engine->sensors.clt, config->cltIdleCorrBins, config->cltIdleCorr, CLT_CURVE_SIZE) / PERCENT_MULT; float iacPosition; diff --git a/firmware/controllers/math/engine_math.cpp b/firmware/controllers/math/engine_math.cpp index e6d91b820c..af3949409d 100644 --- a/firmware/controllers/math/engine_math.cpp +++ b/firmware/controllers/math/engine_math.cpp @@ -241,7 +241,7 @@ floatms_t getSparkDwell(int rpm DECLARE_ENGINE_PARAMETER_SUFFIX) { } else { efiAssert(!cisnan(rpm), "invalid rpm", NAN); - dwellMs = interpolate2d(rpm, engineConfiguration->sparkDwellRpmBins, engineConfiguration->sparkDwellValues, DWELL_CURVE_SIZE); + dwellMs = interpolate2d("dwell", rpm, engineConfiguration->sparkDwellRpmBins, engineConfiguration->sparkDwellValues, DWELL_CURVE_SIZE); } if (cisnan(dwellMs) || dwellMs <= 0) { diff --git a/unit_tests/test_basic_math/test_find_index.cpp b/unit_tests/test_basic_math/test_find_index.cpp index 7ee0358903..618d54dca1 100644 --- a/unit_tests/test_basic_math/test_find_index.cpp +++ b/unit_tests/test_basic_math/test_find_index.cpp @@ -81,19 +81,19 @@ void testInterpolate2d(void) { int result; printf("Left size\r\n"); - result = interpolate2d(0, bins4, values4, size); + result = interpolate2d("t", 0, bins4, values4, size); assertEquals(1, result); printf("Right size\r\n"); - result = interpolate2d(10, bins4, values4, size); + result = interpolate2d("t", 10, bins4, values4, size); assertEquals(400, result); printf("Middle1\r\n"); - result = interpolate2d(3, bins4, values4, size); + result = interpolate2d("t", 3, bins4, values4, size); assertEquals(30, result); printf("Middle1\r\n"); - result = interpolate2d(3.5, bins4, values4, size); + result = interpolate2d("t", 3.5, bins4, values4, size); assertEquals(215, result); }