Performance: 15%+ loop/sec speedup by optimising 3d table bilinear interpolation (#735)
* Bi-linear interpolation - round towards nearest integer Add the equivalent of 0.5 to the final calculation pre-rounding. This will have the effect of rounding to the nearest integer, rather than truncating. I.e. rounding down * Unit tests: isolate table tests and check interpolation x/y bins * Unit tests: add native table3d tests * Unit tests: derive min/max from axis test values * Unit tests: more detailed messages * Unit tests: fix rounding & unit tests * Performance * Performance: use uint16_t instead of unsigned long for the fixed point math: we only need the fractional part. I.e. 1.16 not 16.16 * Use narrowest possible type * Optimise bin position logic for performance * Only promote to uint32_t when really required. * Simplify bin checks - no zero width bins * Save memory: use a single byte for the last bin caches * Performance: increment pointers instead of repeatedly dereferncing array by index. * Comments * Incorrect array underrun logic * Rename type, comments * Fix unit tests
This commit is contained in:
parent
9c390deacb
commit
9ad500189b
|
@ -17,6 +17,7 @@ build_flags = -O3 -ffast-math -fshort-enums -funroll-loops -Wall -Wextra -std=c9
|
|||
lib_deps = EEPROM, Time
|
||||
test_build_project_src = true
|
||||
debug_tool = simavr
|
||||
test_ignore = test_table3d_native
|
||||
|
||||
[env:megaatmega2561]
|
||||
platform=atmelavr
|
||||
|
@ -158,4 +159,9 @@ default_envs = megaatmega2560
|
|||
;env_default = bluepill_f103c8
|
||||
;env_default = black_F401CC
|
||||
|
||||
|
||||
[env:native]
|
||||
platform = native
|
||||
build_flags = -std=gnu++11
|
||||
test_ignore = test_misc, test_decoders, test_schedules
|
||||
debug_test = test_table3d_native
|
||||
build_type = debug
|
|
@ -1,225 +1,211 @@
|
|||
#include "table3d_interpolate.h"
|
||||
|
||||
//The shift amount used for the 3D table calculations
|
||||
#define TABLE_SHIFT_FACTOR 8
|
||||
#define TABLE_SHIFT_POWER (1UL<<TABLE_SHIFT_FACTOR)
|
||||
|
||||
// ============================= Axis Bin Searching =========================
|
||||
|
||||
static inline bool is_in_bin(const table3d_axis_t &testValue, const table3d_axis_t &min, const table3d_axis_t &max)
|
||||
{
|
||||
return testValue > min && testValue <= max;
|
||||
}
|
||||
|
||||
// Find the axis index for the top of the bin that covers the test value.
|
||||
// E.g. 4 in { 1, 3, 5, 7, 9 } would be 2
|
||||
// We assume the axis is in order.
|
||||
static inline table3d_dim_t find_bin_max(
|
||||
table3d_axis_t &value, // Value to search for
|
||||
const table3d_axis_t *pAxis, // The axis to search
|
||||
table3d_dim_t minElement, // Axis index of the element with the lowest value (at one end of the array)
|
||||
table3d_dim_t maxElement, // Axis index of the element with the highest value (at the other end of the array)
|
||||
table3d_dim_t lastBinMax) // The last result from this call - used to speed up searches
|
||||
{
|
||||
// Direction to search (1 coventional, -1 to go backwards from pAxis)
|
||||
int8_t stride = maxElement>minElement ? 1 : -1;
|
||||
// It's quicker to increment/adjust this pointer than to repeatedly
|
||||
// index the array - minimum 2%, often >5%
|
||||
const table3d_axis_t *pMax = nullptr;
|
||||
// minElement is at one end of the array, so the "lowest" bin
|
||||
// is [minElement, minElement+stride]. Since we're working with the upper
|
||||
// index of the bin pair, we can't go below minElement + stride.
|
||||
table3d_dim_t minBinIndex = minElement + stride;
|
||||
|
||||
// Check the cached last bin and either side first - it's likely that this will give a hit under
|
||||
// real world conditions
|
||||
|
||||
// Check if we're still in the same bin as last time
|
||||
pMax = pAxis + lastBinMax;
|
||||
if (is_in_bin(value, *(pMax - stride), *pMax))
|
||||
{
|
||||
return lastBinMax;
|
||||
}
|
||||
// Check the bin above the last one
|
||||
pMax = pMax - stride;
|
||||
if (lastBinMax!=minBinIndex && is_in_bin(value, *(pMax - stride), *pMax))
|
||||
{
|
||||
return lastBinMax-stride;
|
||||
}
|
||||
// Check the bin below the last one
|
||||
pMax += stride*2;
|
||||
if (lastBinMax!=maxElement && is_in_bin(value, *(pMax - stride), *pMax))
|
||||
{
|
||||
return lastBinMax+stride;
|
||||
}
|
||||
|
||||
// Check if outside array limits - won't happen often in the real world
|
||||
// so check after the cache check
|
||||
// At or above maximum - clamp to final value
|
||||
if (value>=pAxis[maxElement])
|
||||
{
|
||||
value = pAxis[maxElement];
|
||||
return maxElement;
|
||||
}
|
||||
// At or below minimum - clamp to lowest value
|
||||
if (value<=pAxis[minElement])
|
||||
{
|
||||
value = pAxis[minElement];
|
||||
return minElement+stride;
|
||||
}
|
||||
|
||||
// No hits above, so run a linear search.
|
||||
// We start at the maximum & work down, rather than looping from [0] up to [max]
|
||||
// This is because the important tables (fuel and injection) will have the highest
|
||||
// RPM at the top of the X axis, so starting there will mean the best case occurs
|
||||
// when the RPM is highest (and hence the CPU is needed most)
|
||||
lastBinMax = maxElement;
|
||||
pMax = pAxis + lastBinMax;
|
||||
while (lastBinMax!=minBinIndex && !is_in_bin(value, *(pMax - stride), *pMax))
|
||||
{
|
||||
lastBinMax -= stride;
|
||||
pMax -= stride;
|
||||
}
|
||||
return lastBinMax;
|
||||
}
|
||||
|
||||
table3d_dim_t find_xbin(table3d_axis_t &value, const table3d_axis_t *pAxis, table3d_dim_t size, table3d_dim_t lastBin)
|
||||
{
|
||||
return find_bin_max(value, pAxis, 0, size-1, lastBin);
|
||||
}
|
||||
|
||||
table3d_dim_t find_ybin(table3d_axis_t &value, const table3d_axis_t *pAxis, table3d_dim_t size, table3d_dim_t lastBin)
|
||||
{
|
||||
// Y axis is stored in reverse for performance purposes (not sure that's still valid).
|
||||
// The minimum value is at the end & max at the start. So need to adjust for that.
|
||||
return find_bin_max(value, pAxis, size-1, 0, lastBin);
|
||||
}
|
||||
|
||||
// ========================= Fixed point math =========================
|
||||
|
||||
// An unsigned fixed point number type with 1 integer bit & 8 fractional bits.
|
||||
// See https://en.wikipedia.org/wiki/Q_(number_format).
|
||||
// This is specialized for the number range 0..1 - a generic fixed point
|
||||
// class would miss some important optimizations. Specifically, we can avoid
|
||||
// type promotion during multiplication.
|
||||
typedef uint16_t QU1X8_t;
|
||||
static constexpr uint8_t QU1X8_INTEGER_SHIFT = 8;
|
||||
static constexpr QU1X8_t QU1X8_ONE = 1U << QU1X8_INTEGER_SHIFT;
|
||||
static constexpr QU1X8_t QU1X8_HALF = 1U << (QU1X8_INTEGER_SHIFT-1);
|
||||
|
||||
inline QU1X8_t mulQU1X8(QU1X8_t a, QU1X8_t b)
|
||||
{
|
||||
// 1x1 == 1....but the real reason for this is to avoid 16-bit multiplication overflow.
|
||||
//
|
||||
// We are using uint16_t as our underlying fixed point type. If we follow the regular
|
||||
// code path, we'd need to promote to uint32_t to avoid overflow.
|
||||
//
|
||||
// The overflow can only happen when *both* the X & Y inputs
|
||||
// are at the edge of a bin.
|
||||
//
|
||||
// This is a rare condition, so most of the time we can use 16-bit mutiplication and gain performance
|
||||
if (a==QU1X8_ONE && b==QU1X8_ONE)
|
||||
{
|
||||
return QU1X8_ONE;
|
||||
}
|
||||
// Add the equivalent of 0.5 to the final calculation pre-rounding.
|
||||
// This will have the effect of rounding to the nearest integer, rather
|
||||
// than always rounding down.
|
||||
return ((a * b) + QU1X8_HALF) >> QU1X8_INTEGER_SHIFT;
|
||||
}
|
||||
|
||||
// ============================= Axis value to bin % =========================
|
||||
|
||||
static inline QU1X8_t compute_bin_position(table3d_axis_t value, const table3d_dim_t &bin, int8_t stride, const table3d_axis_t *pAxis)
|
||||
{
|
||||
table3d_axis_t binMinValue = pAxis[bin-stride];
|
||||
if (value==binMinValue) { return 0; }
|
||||
table3d_axis_t binMaxValue = pAxis[bin];
|
||||
if (value==binMaxValue) { return QU1X8_ONE; }
|
||||
table3d_axis_t binWidth = binMaxValue-binMinValue;
|
||||
|
||||
// Since we can have bins of any width, we need to use
|
||||
// 24.8 fixed point to avoid overflow
|
||||
uint32_t p = (uint32_t)(value - binMinValue) << QU1X8_INTEGER_SHIFT;
|
||||
// But since we are computing the ratio (0 to 1), p is guarenteed to be
|
||||
// less than binWidth and thus the division below will result in a value
|
||||
// <=1. So we can reduce the data type from 24.8 (uint32_t) to 1.8 (uint16_t)
|
||||
return p / binWidth;
|
||||
}
|
||||
|
||||
|
||||
// ============================= End internal support functions =========================
|
||||
|
||||
//This function pulls a value from a 3D table given a target for X and Y coordinates.
|
||||
//It performs a 2D linear interpolation as descibred in: www.megamanual.com/v22manual/ve_tuner.pdf
|
||||
table3d_value_t get3DTableValue(struct table3DGetValueCache *fromTable,
|
||||
//It performs a 2D linear interpolation as described in: www.megamanual.com/v22manual/ve_tuner.pdf
|
||||
table3d_value_t get3DTableValue(struct table3DGetValueCache *pValueCache,
|
||||
table3d_dim_t axisSize,
|
||||
const table3d_value_t *pValues,
|
||||
const table3d_axis_t *pXAxis,
|
||||
const table3d_axis_t *pYAxis,
|
||||
table3d_axis_t Y_in, table3d_axis_t X_in)
|
||||
{
|
||||
table3d_axis_t X = X_in;
|
||||
table3d_axis_t Y = Y_in;
|
||||
|
||||
table3d_value_t tableResult = 0;
|
||||
//Loop through the X axis bins for the min/max pair
|
||||
//Note: For the X axis specifically, rather than looping from tableAxisX[0] up to tableAxisX[max], we start at tableAxisX[Max] and go down.
|
||||
// This is because the important tables (fuel and injection) will have the highest RPM at the top of the X axis, so starting there will mean the best case occurs when the RPM is highest (And hence the CPU is needed most)
|
||||
table3d_axis_t xMinValue = pXAxis[0];
|
||||
table3d_axis_t xMaxValue = pXAxis[axisSize-1];
|
||||
table3d_axis_t xMin = 0;
|
||||
table3d_axis_t xMax = 0;
|
||||
|
||||
//If the requested X value is greater/small than the maximum/minimum bin, reset X to be that value
|
||||
if(X > xMaxValue) { X = xMaxValue; }
|
||||
if(X < xMinValue) { X = xMinValue; }
|
||||
|
||||
//0th check is whether the same X and Y values are being sent as last time. If they are, this not only prevents a lookup of the axis, but prevents the interpolation calcs being performed
|
||||
if( (X_in == fromTable->lastXInput) && (Y_in == fromTable->lastYInput))
|
||||
{
|
||||
//0th check is whether the same X and Y values are being sent as last time.
|
||||
// If they are, this not only prevents a lookup of the axis, but prevents the
|
||||
//interpolation calcs being performed
|
||||
if( X_in == pValueCache->last_lookup.x &&
|
||||
Y_in == pValueCache->last_lookup.y)
|
||||
{
|
||||
return fromTable->lastOutput;
|
||||
return pValueCache->lastOutput;
|
||||
}
|
||||
|
||||
//Commence the lookups on the X and Y axis
|
||||
|
||||
//1st check is whether we're still in the same X bin as last time
|
||||
if ( (X <= pXAxis[fromTable->lastXMax]) && (X > pXAxis[fromTable->lastXMin]) )
|
||||
{
|
||||
xMaxValue = pXAxis[fromTable->lastXMax];
|
||||
xMinValue = pXAxis[fromTable->lastXMin];
|
||||
xMax = fromTable->lastXMax;
|
||||
xMin = fromTable->lastXMin;
|
||||
}
|
||||
//2nd check is whether we're in the next RPM bin (To the right)
|
||||
else if ( ((fromTable->lastXMax + 1) < axisSize ) && (X <= pXAxis[fromTable->lastXMax +1 ]) && (X > pXAxis[fromTable->lastXMin + 1]) ) //First make sure we're not already at the last X bin
|
||||
{
|
||||
xMax = fromTable->lastXMax + 1;
|
||||
fromTable->lastXMax = xMax;
|
||||
xMin = fromTable->lastXMin + 1;
|
||||
fromTable->lastXMin = xMin;
|
||||
xMaxValue = pXAxis[fromTable->lastXMax];
|
||||
xMinValue = pXAxis[fromTable->lastXMin];
|
||||
}
|
||||
//3rd check is to look at the previous bin (to the left)
|
||||
else if ( (fromTable->lastXMin > 0 ) && (X <= pXAxis[fromTable->lastXMax - 1]) && (X > pXAxis[fromTable->lastXMin - 1]) ) //First make sure we're not already at the first X bin
|
||||
{
|
||||
xMax = fromTable->lastXMax - 1;
|
||||
fromTable->lastXMax = xMax;
|
||||
xMin = fromTable->lastXMin - 1;
|
||||
fromTable->lastXMin = xMin;
|
||||
xMaxValue = pXAxis[fromTable->lastXMax];
|
||||
xMinValue = pXAxis[fromTable->lastXMin];
|
||||
}
|
||||
else
|
||||
//If it's not caught by one of the above scenarios, give up and just run the loop
|
||||
{
|
||||
for (int8_t x = axisSize-1; x >= 0; x--)
|
||||
{
|
||||
//Checks the case where the X value is exactly what was requested
|
||||
if ( (X == pXAxis[x]) || (x == 0) )
|
||||
{
|
||||
xMaxValue = pXAxis[x];
|
||||
xMinValue = pXAxis[x];
|
||||
xMax = x;
|
||||
fromTable->lastXMax = xMax;
|
||||
xMin = x;
|
||||
fromTable->lastXMin = xMin;
|
||||
break;
|
||||
}
|
||||
//Normal case
|
||||
if ( (X <= pXAxis[x]) && (X > pXAxis[x-1]) )
|
||||
{
|
||||
xMaxValue = pXAxis[x];
|
||||
xMinValue = pXAxis[x-1];
|
||||
xMax = x;
|
||||
fromTable->lastXMax = xMax;
|
||||
xMin = x-1;
|
||||
fromTable->lastXMin = xMin;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//Loop through the Y axis bins for the min/max pair
|
||||
table3d_axis_t yMaxValue = pYAxis[0];
|
||||
table3d_axis_t yMinValue = pYAxis[axisSize-1];
|
||||
table3d_axis_t yMin = 0;
|
||||
table3d_axis_t yMax = 0;
|
||||
|
||||
//If the requested Y value is greater/small than the maximum/minimum bin, reset Y to be that value
|
||||
if(Y > yMaxValue) { Y = yMaxValue; }
|
||||
if(Y < yMinValue) { Y = yMinValue; }
|
||||
|
||||
//1st check is whether we're still in the same Y bin as last time
|
||||
if ( (Y >= pYAxis[fromTable->lastYMax]) && (Y < pYAxis[fromTable->lastYMin]) )
|
||||
{
|
||||
yMaxValue = pYAxis[fromTable->lastYMax];
|
||||
yMinValue = pYAxis[fromTable->lastYMin];
|
||||
yMax = fromTable->lastYMax;
|
||||
yMin = fromTable->lastYMin;
|
||||
}
|
||||
//2nd check is whether we're in the next MAP/TPS bin (Next one up)
|
||||
else if ( (fromTable->lastYMin > 0 ) && (Y <= pYAxis[fromTable->lastYMin - 1 ]) && (Y > pYAxis[fromTable->lastYMax - 1]) ) //First make sure we're not already at the top Y bin
|
||||
{
|
||||
yMax = fromTable->lastYMax - 1;
|
||||
fromTable->lastYMax = yMax;
|
||||
yMin = fromTable->lastYMin - 1;
|
||||
fromTable->lastYMin = yMin;
|
||||
yMaxValue = pYAxis[fromTable->lastYMax];
|
||||
yMinValue = pYAxis[fromTable->lastYMin];
|
||||
}
|
||||
//3rd check is to look at the previous bin (Next one down)
|
||||
else if ( ((fromTable->lastYMax + 1) < axisSize) && (Y <= pYAxis[fromTable->lastYMin + 1]) && (Y > pYAxis[fromTable->lastYMax + 1]) ) //First make sure we're not already at the bottom Y bin
|
||||
{
|
||||
yMax = fromTable->lastYMax + 1;
|
||||
fromTable->lastYMax = yMax;
|
||||
yMin = fromTable->lastYMin + 1;
|
||||
fromTable->lastYMin = yMin;
|
||||
yMaxValue = pYAxis[fromTable->lastYMax];
|
||||
yMinValue = pYAxis[fromTable->lastYMin];
|
||||
}
|
||||
else
|
||||
//If it's not caught by one of the above scenarios, give up and just run the loop
|
||||
{
|
||||
|
||||
for (int8_t y = axisSize-1; y >= 0; y--)
|
||||
{
|
||||
//Checks the case where the Y value is exactly what was requested
|
||||
if ( (Y == pYAxis[y]) || (y==0) )
|
||||
{
|
||||
yMaxValue = pYAxis[y];
|
||||
yMinValue = pYAxis[y];
|
||||
yMax = y;
|
||||
fromTable->lastYMax = yMax;
|
||||
yMin = y;
|
||||
fromTable->lastYMin = yMin;
|
||||
break;
|
||||
}
|
||||
//Normal case
|
||||
if ( (Y >= pYAxis[y]) && (Y < pYAxis[y-1]) )
|
||||
{
|
||||
yMaxValue = pYAxis[y];
|
||||
yMinValue = pYAxis[y-1];
|
||||
yMax = y;
|
||||
fromTable->lastYMax = yMax;
|
||||
yMin = y-1;
|
||||
fromTable->lastYMin = yMin;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
// Assign this here, as we might modify coords below.
|
||||
pValueCache->last_lookup.x = X_in;
|
||||
pValueCache->last_lookup.y = Y_in;
|
||||
|
||||
// Figure out where on the axes the incoming coord are
|
||||
pValueCache->lastXBinMax = find_xbin(X_in, pXAxis, axisSize, pValueCache->lastXBinMax);
|
||||
pValueCache->lastYBinMax = find_ybin(Y_in, pYAxis, axisSize, pValueCache->lastYBinMax);
|
||||
|
||||
/*
|
||||
At this point we have the 4 corners of the map where the interpolated value will fall in
|
||||
Eg: (yMin,xMin) (yMin,xMax)
|
||||
Eg: (yMax,xMin) (yMax,xMax)
|
||||
|
||||
(yMax,xMin) (yMax,xMax)
|
||||
(yMin,xMin) (yMin,xMax)
|
||||
|
||||
In the following calculation the table values are referred to by the following variables:
|
||||
A B
|
||||
|
||||
C D
|
||||
|
||||
*/
|
||||
table3d_axis_t A = pValues[(yMin*axisSize)+xMin];
|
||||
table3d_axis_t B = pValues[(yMin*axisSize)+xMax];
|
||||
table3d_axis_t C = pValues[(yMax*axisSize)+xMin];
|
||||
table3d_axis_t D = pValues[(yMax*axisSize)+xMax];
|
||||
table3d_dim_t rowMax = pValueCache->lastYBinMax * axisSize;
|
||||
table3d_dim_t rowMin = (pValueCache->lastYBinMax+1) * axisSize;
|
||||
table3d_value_t A = pValues[rowMax + pValueCache->lastXBinMax-1];
|
||||
table3d_value_t B = pValues[rowMax + pValueCache->lastXBinMax];
|
||||
table3d_value_t C = pValues[rowMin + pValueCache->lastXBinMax-1];
|
||||
table3d_value_t D = pValues[rowMin + pValueCache->lastXBinMax];
|
||||
|
||||
//Check that all values aren't just the same (This regularly happens with things like the fuel trim maps)
|
||||
if( (A == B) && (A == C) && (A == D) ) { tableResult = A; }
|
||||
if( (A == B) && (A == C) && (A == D) ) { pValueCache->lastOutput = A; }
|
||||
else
|
||||
{
|
||||
//Create some normalised position values
|
||||
//These are essentially percentages (between 0 and 1) of where the desired value falls between the nearest bins on each axis
|
||||
const QU1X8_t p = compute_bin_position(X_in, pValueCache->lastXBinMax, 1, pXAxis);
|
||||
const QU1X8_t q = compute_bin_position(Y_in, pValueCache->lastYBinMax, -1, pYAxis);
|
||||
|
||||
|
||||
//Initial check incase the values were hit straight on
|
||||
|
||||
unsigned long p = (long)X - xMinValue;
|
||||
if (xMaxValue == xMinValue) { p = (p << TABLE_SHIFT_FACTOR); } //This only occurs if the requested X value was equal to one of the X axis bins
|
||||
else { p = ( (p << TABLE_SHIFT_FACTOR) / (xMaxValue - xMinValue) ); } //This is the standard case
|
||||
|
||||
unsigned long q;
|
||||
if (yMaxValue == yMinValue)
|
||||
{
|
||||
q = (long)Y - yMinValue;
|
||||
q = (q << TABLE_SHIFT_FACTOR);
|
||||
}
|
||||
//Standard case
|
||||
else
|
||||
{
|
||||
q = long(Y) - yMaxValue;
|
||||
q = TABLE_SHIFT_POWER - ( (q << TABLE_SHIFT_FACTOR) / (yMinValue - yMaxValue) );
|
||||
}
|
||||
|
||||
uint32_t m = ((TABLE_SHIFT_POWER-p) * (TABLE_SHIFT_POWER-q)) >> TABLE_SHIFT_FACTOR;
|
||||
uint32_t n = (p * (TABLE_SHIFT_POWER-q)) >> TABLE_SHIFT_FACTOR;
|
||||
uint32_t o = ((TABLE_SHIFT_POWER-p) * q) >> TABLE_SHIFT_FACTOR;
|
||||
uint32_t r = (p * q) >> TABLE_SHIFT_FACTOR;
|
||||
tableResult = ( (A * m) + (B * n) + (C * o) + (D * r) ) >> TABLE_SHIFT_FACTOR;
|
||||
const QU1X8_t m = mulQU1X8(QU1X8_ONE-p, q);
|
||||
const QU1X8_t n = mulQU1X8(p, q);
|
||||
const QU1X8_t o = mulQU1X8(QU1X8_ONE-p, QU1X8_ONE-q);
|
||||
const QU1X8_t r = mulQU1X8(p, QU1X8_ONE-q);
|
||||
pValueCache->lastOutput = ( (A * m) + (B * n) + (C * o) + (D * r) ) >> QU1X8_INTEGER_SHIFT;
|
||||
}
|
||||
|
||||
//Update the tables cache data
|
||||
fromTable->lastXInput = X_in;
|
||||
fromTable->lastYInput = Y_in;
|
||||
fromTable->lastOutput = tableResult;
|
||||
|
||||
return tableResult;
|
||||
}
|
||||
return pValueCache->lastOutput;
|
||||
}
|
|
@ -2,19 +2,40 @@
|
|||
|
||||
#include "table3d_typedefs.h"
|
||||
|
||||
// A table location.
|
||||
struct coord2d
|
||||
{
|
||||
table3d_axis_t x;
|
||||
table3d_axis_t y;
|
||||
};
|
||||
|
||||
|
||||
struct table3DGetValueCache {
|
||||
//Store the last X and Y coordinates in the table. This is used to make the next check faster
|
||||
table3d_dim_t lastXMax, lastXMin;
|
||||
table3d_dim_t lastYMax, lastYMin;
|
||||
// Store the upper *index* of the X and Y axis bins that were last hit.
|
||||
// This is used to make the next check faster since very likely the x & y values have
|
||||
// only changed by a small amount & are in the same bin (or an adjacent bin).
|
||||
//
|
||||
// It's implicit that the other bin index is max bin index - 1 (a single axis
|
||||
// value can't span 2 axis bins). This saves 1 byte.
|
||||
//
|
||||
// E.g. 6 element x-axis contents:
|
||||
// [ 8| 9|12|15|18|21]
|
||||
// indices:
|
||||
// 0, 1, 2, 3, 4, 5
|
||||
// If lastXBinMax==3, the min index must be 2. I.e. the last X value looked
|
||||
// up was between 12<X<=15.
|
||||
table3d_dim_t lastXBinMax = 1;
|
||||
table3d_dim_t lastYBinMax = 1;
|
||||
|
||||
//Store the last input and output values, again for caching purposes
|
||||
table3d_axis_t lastXInput = INT16_MAX, lastYInput;
|
||||
table3d_value_t lastOutput; // This will need changing if we ever have 16-bit table values
|
||||
coord2d last_lookup = { INT16_MAX, INT16_MAX };
|
||||
table3d_value_t lastOutput;
|
||||
};
|
||||
|
||||
|
||||
inline void invalidate_cache(table3DGetValueCache *pCache)
|
||||
{
|
||||
pCache->lastXInput = INT16_MAX;
|
||||
pCache->last_lookup.x = INT16_MAX;
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
|
@ -1,28 +1,46 @@
|
|||
#include <globals.h>
|
||||
#include <init.h>
|
||||
//#include <Arduino.h>
|
||||
#include <string.h> // memcpy
|
||||
#include <unity.h>
|
||||
#include <stdio.h>
|
||||
#include "tests_tables.h"
|
||||
#include "table3d.h"
|
||||
|
||||
#define _countof(x) (sizeof(x) / sizeof (x[0]))
|
||||
|
||||
#if defined(PROGMEM)
|
||||
const PROGMEM byte values[] = {
|
||||
109, 111, 112, 113, 114, 114, 114, 115, 115, 115, 114, 114, 113, 112, 111, 111,
|
||||
104, 106, 107, 108, 109, 109, 110, 110, 110, 110, 110, 109, 108, 107, 107, 106,
|
||||
98, 101, 103, 103, 104, 105, 105, 105, 105, 105, 105, 104, 104, 103, 102, 102,
|
||||
93, 96, 98, 99, 99, 100, 100, 101, 101, 101, 100, 100, 99, 98, 98, 97,
|
||||
81, 86, 88, 89, 90, 91, 91, 91, 91, 91, 91, 90, 90, 89, 89, 88,
|
||||
74, 80, 83, 84, 85, 86, 86, 86, 87, 86, 86, 86, 85, 84, 84, 84,
|
||||
68, 75, 78, 79, 81, 81, 81, 82, 82, 82, 82, 81, 81, 80, 79, 79,
|
||||
61, 69, 72, 74, 76, 76, 77, 77, 77, 77, 77, 76, 76, 75, 75, 74,
|
||||
54, 62, 66, 69, 71, 71, 72, 72, 72, 72, 72, 72, 71, 71, 70, 70,
|
||||
48, 56, 60, 64, 66, 66, 68, 68, 68, 68, 67, 67, 67, 66, 66, 65,
|
||||
42, 49, 54, 58, 61, 62, 62, 63, 63, 63, 63, 62, 62, 61, 61, 61,
|
||||
38, 43, 48, 52, 55, 56, 57, 58, 58, 58, 58, 58, 57, 57, 57, 56,
|
||||
36, 39, 42, 46, 50, 51, 52, 53, 53, 53, 53, 53, 53, 52, 52, 52,
|
||||
35, 36, 38, 41, 44, 46, 47, 48, 48, 49, 48, 48, 48, 48, 47, 47,
|
||||
34, 35, 36, 37, 39, 41, 42, 43, 43, 44, 44, 44, 43, 43, 43, 43,
|
||||
34, 34, 34, 34, 34, 34, 34, 34, 34, 35, 34, 34, 34, 34, 34, 34
|
||||
#else
|
||||
const byte values[] = {
|
||||
#endif
|
||||
//0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15
|
||||
109, 111, 112, 113, 114, 114, 114, 115, 115, 115, 114, 114, 114, 114, 114, 114,
|
||||
104, 106, 107, 108, 109, 109, 110, 110, 110, 110, 110, 110, 110, 110, 110, 110,
|
||||
98, 101, 103, 103, 104, 105, 105, 105, 105, 105, 105, 105, 105, 105, 105, 105,
|
||||
93, 96, 98, 99, 99, 100, 100, 101, 101, 101, 101, 101, 101, 101, 101, 101,
|
||||
81, 86, 88, 89, 90, 91, 91, 91, 91, 91, 91, 91, 91, 91, 91, 91,
|
||||
74, 80, 83, 84, 85, 86, 86, 86, 87, 87, 87, 87, 87, 87, 87, 87,
|
||||
68, 75, 78, 79, 81, 81, 81, 82, 82, 82, 82, 82, 82, 82, 82, 82,
|
||||
61, 69, 72, 74, 76, 76, 77, 77, 77, 77, 77, 77, 77, 77, 77, 77,
|
||||
54, 62, 66, 69, 71, 71, 72, 72, 72, 72, 72, 72, 72, 72, 72, 72,
|
||||
48, 56, 60, 64, 66, 66, 68, 68, 68, 68, 68, 68, 68, 68, 68, 68,
|
||||
42, 49, 54, 58, 61, 62, 62, 63, 63, 63, 63, 63, 63, 63, 63, 63,
|
||||
38, 43, 48, 52, 55, 56, 57, 58, 58, 58, 58, 58, 58, 58, 58, 58,
|
||||
36, 39, 42, 46, 50, 51, 52, 53, 53, 53, 53, 53, 53, 53, 53, 53,
|
||||
35, 36, 38, 41, 44, 46, 47, 48, 48, 49, 49, 49, 49, 49, 49, 49,
|
||||
34, 35, 36, 37, 39, 41, 42, 43, 43, 44, 44, 44, 44, 44, 44, 44,
|
||||
34, 34, 34, 34, 34, 34, 34, 34, 34, 35, 35, 35, 35, 35, 35, 35,
|
||||
};
|
||||
static const table3d_axis_t tempXAxis[] = {500,700, 900, 1200, 1600, 2000, 2500, 3100, 3500, 4100, 4700, 5300, 5900, 6500, 6750, 7000};
|
||||
static const table3d_axis_t xMin = tempXAxis[0];
|
||||
static const table3d_axis_t xMax = tempXAxis[_countof(tempXAxis)-1];
|
||||
static const table3d_axis_t tempYAxis[] = {100, 96, 90, 86, 76, 70, 66, 60, 56, 50, 46, 40, 36, 30, 26, 16};
|
||||
static const table3d_axis_t yMin = tempYAxis[_countof(tempYAxis)-1];
|
||||
static const table3d_axis_t yMax = tempYAxis[0];
|
||||
|
||||
void setup_FuelTable(void)
|
||||
|
||||
static table3d16RpmLoad testTable;
|
||||
|
||||
void setup_TestTable(void)
|
||||
{
|
||||
//Setup the fuel table with some sane values for testing
|
||||
//Table is setup per the below
|
||||
|
@ -47,15 +65,14 @@ void setup_FuelTable(void)
|
|||
500 | 700 | 900 | 1200 | 1600 | 2000 | 2500 | 3100 | 3500 | 4100 | 4700 | 5300 | 5900 | 6500 | 6750 | 7000
|
||||
*/
|
||||
|
||||
table3d_axis_t tempXAxis[] = {500,700, 900, 1200, 1600, 2000, 2500, 3100, 3500, 4100, 4700, 5300, 5900, 6500, 6750, 7000};
|
||||
memcpy(fuelTable.axisX.axis, tempXAxis, sizeof(fuelTable.axisX));
|
||||
table3d_axis_t tempYAxis[] = {100, 96, 90, 86, 76, 70, 66, 60, 56, 50, 46, 40, 36, 30, 26, 16};
|
||||
memcpy(fuelTable.axisY.axis, tempYAxis, sizeof(fuelTable.axisY));
|
||||
|
||||
for (int loop=0; loop<sizeof(fuelTable.values); ++loop)
|
||||
{
|
||||
fuelTable.values.values[loop] = pgm_read_byte_near(values + loop);
|
||||
}
|
||||
memcpy(testTable.axisX.axis, tempXAxis, sizeof(testTable.axisX));
|
||||
memcpy(testTable.axisY.axis, tempYAxis, sizeof(testTable.axisY));
|
||||
#if defined(PROGMEM)
|
||||
memcpy_P
|
||||
#else
|
||||
memcpy
|
||||
#endif
|
||||
(testTable.values.values, values, sizeof(testTable.values));
|
||||
}
|
||||
|
||||
void testTables()
|
||||
|
@ -67,6 +84,7 @@ void testTables()
|
|||
RUN_TEST(test_tableLookup_overMaxY);
|
||||
RUN_TEST(test_tableLookup_underMinX);
|
||||
RUN_TEST(test_tableLookup_underMinY);
|
||||
RUN_TEST(test_tableLookup_roundUp);
|
||||
//RUN_TEST(test_all_incrementing);
|
||||
|
||||
}
|
||||
|
@ -74,92 +92,91 @@ void testTables()
|
|||
void test_tableLookup_50pct(void)
|
||||
{
|
||||
//Tests a lookup that is exactly 50% of the way between cells on both the X and Y axis
|
||||
//initialiseAll(); //Run the main initialise function
|
||||
setup_FuelTable();
|
||||
setup_TestTable();
|
||||
|
||||
currentStatus.RPM = 2250;
|
||||
currentStatus.fuelLoad = 53;
|
||||
|
||||
uint16_t tempVE = get3DTableValue(&fuelTable, currentStatus.fuelLoad, currentStatus.RPM); //Perform lookup into fuel map for RPM vs MAP value
|
||||
uint16_t tempVE = get3DTableValue(&testTable, 53, 2250); //Perform lookup into fuel map for RPM vs MAP value
|
||||
TEST_ASSERT_EQUAL(tempVE, 69);
|
||||
TEST_ASSERT_EQUAL(testTable.get_value_cache.lastXBinMax, (table3d_dim_t)6);
|
||||
TEST_ASSERT_EQUAL(testTable.get_value_cache.lastYBinMax, (table3d_dim_t)8);
|
||||
}
|
||||
|
||||
void test_tableLookup_exact1Axis(void)
|
||||
{
|
||||
//Tests a lookup that exactly matches on the X axis and 50% of the way between cells on the Y axis
|
||||
initialiseAll(); //Run the main initialise function
|
||||
setup_FuelTable();
|
||||
setup_TestTable();
|
||||
|
||||
currentStatus.RPM = 2500;
|
||||
currentStatus.fuelLoad = 48;
|
||||
|
||||
uint16_t tempVE = get3DTableValue(&fuelTable, currentStatus.fuelLoad, currentStatus.RPM); //Perform lookup into fuel map for RPM vs MAP value
|
||||
uint16_t tempVE = get3DTableValue(&testTable, 48, testTable.axisX.axis[6]); //Perform lookup into fuel map for RPM vs MAP value
|
||||
TEST_ASSERT_EQUAL(tempVE, 65);
|
||||
TEST_ASSERT_EQUAL(testTable.get_value_cache.lastXBinMax, (table3d_dim_t)6);
|
||||
TEST_ASSERT_EQUAL(testTable.get_value_cache.lastYBinMax, (table3d_dim_t)9);
|
||||
}
|
||||
|
||||
void test_tableLookup_exact2Axis(void)
|
||||
{
|
||||
//Tests a lookup that exactly matches on both the X and Y axis
|
||||
initialiseAll(); //Run the main initialise function
|
||||
setup_FuelTable();
|
||||
setup_TestTable();
|
||||
|
||||
currentStatus.RPM = 2500;
|
||||
currentStatus.fuelLoad = 70;
|
||||
|
||||
uint16_t tempVE = get3DTableValue(&fuelTable, currentStatus.fuelLoad, currentStatus.RPM); //Perform lookup into fuel map for RPM vs MAP value
|
||||
uint16_t tempVE = get3DTableValue(&testTable, testTable.axisY.axis[5], testTable.axisX.axis[6]); //Perform lookup into fuel map for RPM vs MAP value
|
||||
TEST_ASSERT_EQUAL(tempVE, 86);
|
||||
TEST_ASSERT_EQUAL(testTable.get_value_cache.lastXBinMax, (table3d_dim_t)6);
|
||||
TEST_ASSERT_EQUAL(testTable.get_value_cache.lastYBinMax, (table3d_dim_t)5);
|
||||
}
|
||||
|
||||
void test_tableLookup_overMaxX(void)
|
||||
{
|
||||
//Tests a lookup where the RPM exceeds the highest value in the table. The Y value is a 50% match
|
||||
initialiseAll(); //Run the main initialise function
|
||||
setup_FuelTable();
|
||||
setup_TestTable();
|
||||
|
||||
currentStatus.RPM = 10000;
|
||||
currentStatus.fuelLoad = 73;
|
||||
|
||||
uint16_t tempVE = get3DTableValue(&fuelTable, currentStatus.fuelLoad, currentStatus.RPM); //Perform lookup into fuel map for RPM vs MAP value
|
||||
TEST_ASSERT_EQUAL(tempVE, 86);
|
||||
uint16_t tempVE = get3DTableValue(&testTable, 73, xMax+100); //Perform lookup into fuel map for RPM vs MAP value
|
||||
TEST_ASSERT_EQUAL(tempVE, 89);
|
||||
TEST_ASSERT_EQUAL(testTable.get_value_cache.lastXBinMax, (table3d_dim_t)15);
|
||||
TEST_ASSERT_EQUAL(testTable.get_value_cache.lastYBinMax, (table3d_dim_t)4);
|
||||
}
|
||||
|
||||
void test_tableLookup_overMaxY(void)
|
||||
{
|
||||
//Tests a lookup where the load value exceeds the highest value in the table. The X value is a 50% match
|
||||
initialiseAll(); //Run the main initialise function
|
||||
setup_FuelTable();
|
||||
setup_TestTable();
|
||||
|
||||
currentStatus.RPM = 600;
|
||||
currentStatus.fuelLoad = 110;
|
||||
|
||||
uint16_t tempVE = get3DTableValue(&fuelTable, currentStatus.fuelLoad, currentStatus.RPM); //Perform lookup into fuel map for RPM vs MAP value
|
||||
uint16_t tempVE = get3DTableValue(&testTable, yMax+10, 600); //Perform lookup into fuel map for RPM vs MAP value
|
||||
TEST_ASSERT_EQUAL(tempVE, 110);
|
||||
TEST_ASSERT_EQUAL(testTable.get_value_cache.lastXBinMax, (table3d_dim_t)1);
|
||||
TEST_ASSERT_EQUAL(testTable.get_value_cache.lastYBinMax, (table3d_dim_t)0);
|
||||
}
|
||||
|
||||
void test_tableLookup_underMinX(void)
|
||||
{
|
||||
//Tests a lookup where the RPM value is below the lowest value in the table. The Y value is a 50% match
|
||||
initialiseAll(); //Run the main initialise function
|
||||
setup_FuelTable();
|
||||
setup_TestTable();
|
||||
|
||||
currentStatus.RPM = 300;
|
||||
currentStatus.fuelLoad = 38;
|
||||
|
||||
uint16_t tempVE = get3DTableValue(&fuelTable, currentStatus.fuelLoad, currentStatus.RPM); //Perform lookup into fuel map for RPM vs MAP value
|
||||
uint16_t tempVE = get3DTableValue(&testTable, 38, xMin-100); //Perform lookup into fuel map for RPM vs MAP value
|
||||
TEST_ASSERT_EQUAL(tempVE, 37);
|
||||
TEST_ASSERT_EQUAL(testTable.get_value_cache.lastXBinMax, (table3d_dim_t)1);
|
||||
TEST_ASSERT_EQUAL(testTable.get_value_cache.lastYBinMax, (table3d_dim_t)11);
|
||||
}
|
||||
|
||||
void test_tableLookup_underMinY(void)
|
||||
{
|
||||
//Tests a lookup where the load value is below the lowest value in the table. The X value is a 50% match
|
||||
initialiseAll(); //Run the main initialise function
|
||||
setup_FuelTable();
|
||||
setup_TestTable();
|
||||
|
||||
currentStatus.RPM = 600;
|
||||
currentStatus.fuelLoad = 8;
|
||||
|
||||
uint16_t tempVE = get3DTableValue(&fuelTable, currentStatus.fuelLoad, currentStatus.RPM); //Perform lookup into fuel map for RPM vs MAP value
|
||||
uint16_t tempVE = get3DTableValue(&testTable, yMin-5, 600); //Perform lookup into fuel map for RPM vs MAP value
|
||||
TEST_ASSERT_EQUAL(tempVE, 34);
|
||||
TEST_ASSERT_EQUAL(testTable.get_value_cache.lastXBinMax, (table3d_dim_t)1);
|
||||
TEST_ASSERT_EQUAL(testTable.get_value_cache.lastYBinMax, (table3d_dim_t)14);
|
||||
}
|
||||
|
||||
void test_tableLookup_roundUp(void)
|
||||
{
|
||||
// Tests a lookup where the inputs result in a value that is outside the table range
|
||||
// due to fixed point rounding
|
||||
// Issue #726
|
||||
setup_TestTable();
|
||||
|
||||
uint16_t tempVE = get3DTableValue(&testTable, 17, 600);
|
||||
TEST_ASSERT_EQUAL(tempVE, 34);
|
||||
TEST_ASSERT_EQUAL(testTable.get_value_cache.lastXBinMax, (table3d_dim_t)1);
|
||||
TEST_ASSERT_EQUAL(testTable.get_value_cache.lastYBinMax, (table3d_dim_t)14);
|
||||
}
|
||||
|
||||
void test_all_incrementing(void)
|
||||
|
@ -169,12 +186,26 @@ void test_all_incrementing(void)
|
|||
//WARNING: This can take a LONG time to run. It is disabled by default for this reason
|
||||
uint16_t tempVE = 0;
|
||||
|
||||
for(uint16_t rpm = 0; rpm<8000; rpm+=100)
|
||||
for(uint16_t rpm = 0; rpm<xMax+1000; rpm+=100)
|
||||
{
|
||||
tempVE = 0;
|
||||
for(uint8_t load = 0; load<120; load++)
|
||||
for(uint8_t load = 0; load<yMax+10; load++)
|
||||
{
|
||||
uint16_t newVE = get3DTableValue(&fuelTable, load, rpm);
|
||||
uint16_t newVE = get3DTableValue(&testTable, load, rpm);
|
||||
// char buffer[256];
|
||||
// sprintf(buffer, "%d, %d"
|
||||
// ", %d, %d, %d, %d"
|
||||
// ", %d, %d, %d, %d"
|
||||
// ", %d",
|
||||
// rpm, load,
|
||||
// testTable.get_value_cache.lastXMin, testTable.get_value_cache.lastXBinMax,
|
||||
// tempXAxis[testTable.get_value_cache.lastXMin], tempXAxis[testTable.get_value_cache.lastXBinMax],
|
||||
|
||||
// testTable.get_value_cache.lastYMin, testTable.get_value_cache.lastYBinMax,
|
||||
// tempYAxis[testTable.get_value_cache.lastYMin], tempYAxis[testTable.get_value_cache.lastYBinMax],
|
||||
|
||||
// newVE);
|
||||
// TEST_MESSAGE(buffer);
|
||||
TEST_ASSERT_GREATER_OR_EQUAL(tempVE, newVE);
|
||||
tempVE = newVE;
|
||||
}
|
||||
|
|
|
@ -7,4 +7,5 @@ void test_tableLookup_overMaxX(void);
|
|||
void test_tableLookup_overMaxY(void);
|
||||
void test_tableLookup_underMinX(void);
|
||||
void test_tableLookup_underMinY(void);
|
||||
void test_all_incrementing(void);
|
||||
void test_tableLookup_roundUp(void);
|
||||
void test_all_incrementing(void);
|
||||
|
|
|
@ -0,0 +1,19 @@
|
|||
#include <unity.h>
|
||||
#include "table3d_interpolate.cpp"
|
||||
#include "..\test_misc\tests_tables.cpp"
|
||||
|
||||
int main(int argc, char **argv) {
|
||||
UNITY_BEGIN();
|
||||
RUN_TEST(test_tableLookup_50pct);
|
||||
RUN_TEST(test_tableLookup_exact1Axis);
|
||||
RUN_TEST(test_tableLookup_exact2Axis);
|
||||
RUN_TEST(test_tableLookup_overMaxX);
|
||||
RUN_TEST(test_tableLookup_overMaxY);
|
||||
RUN_TEST(test_tableLookup_underMinX);
|
||||
RUN_TEST(test_tableLookup_underMinY);
|
||||
RUN_TEST(test_tableLookup_roundUp);
|
||||
RUN_TEST(test_all_incrementing);
|
||||
UNITY_END();
|
||||
|
||||
return 0;
|
||||
}
|
Loading…
Reference in New Issue