ETB: TPS should be f(RPM, pedal) - target table fix #835

firmware/controllers/actuators/electronic_throttle.cpp

@@ -95,6 +95,7 @@ static Pid tuneWorkingPid(&tuneWorkingPidSettings);
 static PID_AutoTune autoTune;
 
 static LoggingWithStorage logger("ETB");
+static pedal2tps_t pedal2tpsMap("Pedal2Tps", 1);
 
 EXTERN_ENGINE;
 
@@ -226,7 +227,18 @@ private:
 		}
 
 
-		percent_t targetPosition = getPedalPosition(PASS_ENGINE_PARAMETER_SIGNATURE) + engine->engineState.etbIdleAddition;
+		percent_t pedalPosition = getPedalPosition(PASS_ENGINE_PARAMETER_SIGNATURE);
+
+		int rpm = GET_RPM();
+		float targetFromTable = pedal2tpsMap.getValue(0/*rpm / RPM_1_BYTE_PACKING_MULT*/, 0/*pedalPosition*/);
+		percent_t targetPosition = targetFromTable + engine->engineState.etbIdleAddition;
+
+		if (engineConfiguration->debugMode == DBG_ETB_LOGIC) {
+#if EFI_TUNER_STUDIO
+			tsOutputChannels.debugFloatField1 = targetFromTable;
+			tsOutputChannels.debugFloatField2 = engine->engineState.etbIdleAddition;
+#endif /* EFI_TUNER_STUDIO */
+		}
 
 		feedForward = interpolate2d("etbb", targetPosition, engineConfiguration->etbBiasBins, engineConfiguration->etbBiasValues, ETB_BIAS_CURVE_LENGTH);
 
@@ -334,7 +346,7 @@ void setEtbOffset(int value) {
 void setDefaultEtbParameters(DECLARE_ENGINE_PARAMETER_SIGNATURE) {
 	CONFIG(etbIdleThrottleRange) = 5;
 
-	setLinearCurveAny<int8_t>(config->pedalToTpsPedalBins, PEDAL_TO_TPS_SIZE, /*from*/0, /*to*/100, 1);
+	setLinearCurveAny<uint8_t>(config->pedalToTpsPedalBins, PEDAL_TO_TPS_SIZE, /*from*/0, /*to*/100, 1);
 	setLinearCurveAny<uint8_t>(config->pedalToTpsRpmBins, PEDAL_TO_TPS_SIZE, /*from*/0, /*to*/8000 / RPM_1_BYTE_PACKING_MULT, 1);
 
 	for (int pedalIndex = 0;pedalIndex<PEDAL_TO_TPS_SIZE;pedalIndex++) {
@@ -452,6 +464,7 @@ void unregisterEtbPins() {
 void initElectronicThrottle(void) {
 	addConsoleAction("ethinfo", showEthInfo);
 	addConsoleAction("etbreset", etbReset);
+	pedal2tpsMap.init(config->pedalToTpsTable, config->pedalToTpsPedalBins, config->pedalToTpsRpmBins);
 	if (!hasPedalPositionSensor()) {
 		return;
 	}

firmware/util/math/interpolation.cpp

@@ -173,64 +173,6 @@ void ensureArrayIsAscending(const char *msg, const float array[], int size) {
 	}
 }
 
-/** @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.
- *
- * See also ensureArrayIsAscending
- */
-int findIndexMsg(const char *msg, const float array[], int size, float value) {
-	if (cisnan(value)) {
-		firmwareError(ERROR_NAN_FIND_INDEX, "NaN in findIndex%s", msg);
-		return 0;
-	}
-
-	if (value < array[0])
-		return -1;
-	int middle;
-
-	int left = 0;
-	int right = size;
-
-	// todo: extract binary search as template method?
-	while (true) {
-#if 0
-		// that's an assertion to make sure we do not loop here
-		size--;
-		efiAssert(CUSTOM_ERR_ASSERT, size > 0, "Unexpected state in binary search", 0);
-#endif
-
-		// todo: compare current implementation with
-		// http://eigenjoy.com/2011/01/21/worlds-fastest-binary-search/
-		// ?
-		middle = (left + right) / 2;
-
-//		print("left=%d middle=%d right=%d: %.2f\r\n", left, middle, right, array[middle]);
-
-		if (middle == left)
-			break;
-
-		if (middle != 0 && array[middle - 1] > array[middle]) {
-#if EFI_UNIT_TEST
-			firmwareError(CUSTOM_ERR_6610, "%s: out of order %.2f %.2f", msg, array[middle - 1], array[middle]);
-#else
-			warning(CUSTOM_ERR_OUT_OF_ORDER, "%s: out of order %.2f %.2f", msg, array[middle - 1], array[middle]);
-
-#endif /* EFI_UNIT_TEST */
-		}
-
-		if (value < array[middle]) {
-			right = middle;
-		} else if (value > array[middle]) {
-			left = middle;
-		} else {
-			break;
-		}
-	}
-
-	return middle;
-}
-
 int findIndex(const float array[], int size, float value) {
 	return findIndexMsg("", array, size, value);
 }

firmware/util/math/interpolation.h

@@ -21,7 +21,7 @@
 #define INTERPOLATION_A(x1, y1, x2, y2) ((y1 - y2) / (x1 - x2))
 
 int findIndex(const float array[], int size, float value);
-int findIndexMsg(const char *msg, const float array[], int size, float value);
+#define findIndexMsg(msg, array, size, value) findIndexMsgExt<float>(msg, array, size, value)
 void ensureArrayIsAscending(const char *msg, const float array[], int size);
 int findIndex2(const float array[], unsigned size, float value);
 float interpolateClamped(float x1, float y1, float x2, float y2, float x);
@@ -30,6 +30,65 @@ float interpolate2d(const char *msg, float value, const float bin[], const float
 
 int needInterpolationLogging(void);
 
+/** @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.
+ *
+ * See also ensureArrayIsAscending
+ */
+template<typename kType>
+int findIndexMsgExt(const char *msg, const kType array[], int size, kType value) {
+	if (cisnan(value)) {
+		firmwareError(ERROR_NAN_FIND_INDEX, "NaN in findIndex%s", msg);
+		return 0;
+	}
+
+	if (value < array[0])
+		return -1;
+	int middle;
+
+	int left = 0;
+	int right = size;
+
+	// todo: extract binary search as template method?
+	while (true) {
+#if 0
+		// that's an assertion to make sure we do not loop here
+		size--;
+		efiAssert(CUSTOM_ERR_ASSERT, size > 0, "Unexpected state in binary search", 0);
+#endif
+
+		// todo: compare current implementation with
+		// http://eigenjoy.com/2011/01/21/worlds-fastest-binary-search/
+		// ?
+		middle = (left + right) / 2;
+
+//		print("left=%d middle=%d right=%d: %.2f\r\n", left, middle, right, array[middle]);
+
+		if (middle == left)
+			break;
+
+		if (middle != 0 && array[middle - 1] > array[middle]) {
+#if EFI_UNIT_TEST
+			firmwareError(CUSTOM_ERR_6610, "%s: out of order %.2f %.2f", msg, array[middle - 1], array[middle]);
+#else
+			warning(CUSTOM_ERR_OUT_OF_ORDER, "%s: out of order %.2f %.2f", msg, array[middle - 1], array[middle]);
+
+#endif /* EFI_UNIT_TEST */
+		}
+
+		if (value < array[middle]) {
+			right = middle;
+		} else if (value > array[middle]) {
+			left = middle;
+		} else {
+			break;
+		}
+	}
+
+	return middle;
+}
+
 /**
  * @brief	Two-dimensional table lookup with linear interpolation
  */
@@ -44,12 +103,12 @@ float interpolate3d(float x, const kType xBin[], int xBinSize, float y, const kT
 		return NAN;
 	}
 
-	int xIndex = findIndexMsg("x", xBin, xBinSize, x);
+	int xIndex = findIndexMsgExt<kType>("x", xBin, xBinSize, x);
 #if	DEBUG_INTERPOLATION
 	if (needInterpolationLogging())
 		printf("X index=%d\r\n", xIndex);
 #endif /* DEBUG_INTERPOLATION */
-	int yIndex = findIndexMsg("y", yBin, yBinSize, y);
+	int yIndex = findIndexMsgExt<kType>("y", yBin, yBinSize, y);
 	if (xIndex < 0 && yIndex < 0) {
 #if	DEBUG_INTERPOLATION
 		if (needInterpolationLogging())