1 files changed, 51 insertions, 23 deletions

diff --git a/libarmep/interpolation.c b/libarmep/interpolation.c
index f27f27d..8afde9a 100644
--- a/libarmep/interpolation.c
+++ b/libarmep/interpolation.c
@@ -884,8 +884,9 @@ struct interp_tables interp_tables[3] = {
 	},
 };
 
-double correct(int no_correction, double common_mode, double in, struct interp *map, int map_len)
+double correct(int no_correction, double common_mode, double in, struct interp_tables *itable)
 {
+	int map_len = itable->len;
 	int n = map_len / 2, m, end;
 	int step = n / 2;
 	int below = -1, above = -1;
@@ -893,29 +894,17 @@ double correct(int no_correction, double common_mode, double in, struct interp *
 	double delta;
 	double factor;
 	double result;
+	struct interp *map = itable->map;
 
 	if (no_correction)
 		return in;
 
-	/* find nearest common-mode entry using binary search */
+	n = (int)(common_mode * 100); /* 10mV steps */
 
-	while (step) {
-		if (map[n].common_mode == common_mode) {
-			step = 0;
-			continue;
-		}
-		if (map[n].common_mode < common_mode)
-			n += step;
-		else
-			n -= step;
+	if (n >= sizeof (itable->precomputed_common_10mV) / sizeof (unsigned short))
+		return in;
 
-		if (n < 0)
-			n = 0;
-		if (n >= map_len)
-			n = map_len - 1;
-
-		step >>= 1;
-	}
+	n = itable->precomputed_common_10mV[n];
 
 	/* from this starting point, find closest bracketing Vshunt raw */
 
@@ -949,14 +938,53 @@ double correct(int no_correction, double common_mode, double in, struct interp *
 	if (above < 0)
 		return in;
 
-	if (below != above) {
+	if (below == above)
+		return map[below].actual;
 
-		factor = (in - map[below].raw) / (map[above].raw - map[below].raw);
+	factor = (in - map[below].raw) / (map[above].raw - map[below].raw);
 
-		result = map[below].actual + (factor * (map[above].actual - map[below].actual));
-	} else
-		result = map[below].actual;
+	result = map[below].actual + (factor * (map[above].actual - map[below].actual));
 
 	return result;
 }
 
+void init_interpolation(void)
+{
+	int n, i, m, step, map_len;
+	double common_mode;
+
+	for (n = 0; n < sizeof(interp_tables) / sizeof(interp_tables[0]); n++) {
+
+		for (i = 0; i < sizeof(interp_tables[0].precomputed_common_10mV) / sizeof (unsigned short); i++) {
+
+			common_mode = (double)i * 0.01;
+			map_len = interp_tables[n].len;
+			m = map_len / 2;
+			step = m / 2;
+
+			/* find nearest common-mode entry using binary search */
+
+			while (step) {
+				if (interp_tables[n].map[m].common_mode == common_mode) {
+					step = 0;
+					continue;
+				}
+				if (interp_tables[n].map[m].common_mode < common_mode)
+					m += step;
+				else
+					m -= step;
+
+				if (m < 0)
+					m = 0;
+				if (m >= map_len)
+					m = map_len - 1;
+
+				step >>= 1;
+			}
+
+			interp_tables[n].precomputed_common_10mV[i] = m;
+		}
+	}
+}
+
+