optimize correction

Signed-off-by: Andy Green <andy.green@linaro.org>

3 files changed, 60 insertions, 29 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;
+		}
+	}
+}
+
+
diff --git a/libarmep/libarmep.h b/libarmep/libarmep.h
index 5bb868c..06b7a33 100644
--- a/libarmep/libarmep.h
+++ b/libarmep/libarmep.h
@@ -41,7 +41,7 @@
 #define AEP_INPUT_QUEUE 163840
 
 #define MAX_RESULT_QUEUE_DEPTH (512 * 8)
-#define MAX_BYTES_PER_AEP_SERVICE 64
+#define MAX_BYTES_PER_AEP_SERVICE 512
 
 #define TRIG_HYSTERESIS_MV 200
 #define TRIG_HYSTERESIS_MW 200
@@ -142,6 +142,7 @@ struct interp_tables {
         struct interp *map;
         int len;
         double percentage_error_ref;
+	unsigned short precomputed_common_10mV[600];
 };
 
 
@@ -283,7 +284,7 @@ extern void avg_mean_us_free(struct avg_mean_us *amu);
 extern void avg_mean_us_flush(struct avg_mean_us *amu);
 
 extern struct interp_tables interp_tables[3];
-extern double correct(int no_correction, double common_mode, double in, struct interp *map, int map_len);
+extern double correct(int no_correction, double common_mode, double in, struct interp_tables *itable);
 
 extern int get_stdin_line(char *buf, int maxlen);
 extern int process_sample(struct aep *aep, int ch_index);
@@ -293,3 +294,4 @@ extern int configure(struct aep_context *aep_context, struct aep *aep, const cha
 extern char configuration_name[64];
 extern int service_aeps(struct aep_context *aep_context, int fd);
 extern void probe_close(struct aep *aep);
+extern void init_interpolation(void);
diff --git a/libarmep/sample.c b/libarmep/sample.c
index 9c9bb3d..d28b038 100644
--- a/libarmep/sample.c
+++ b/libarmep/sample.c
@@ -50,7 +50,7 @@ static void track_limits_convert_to_current(struct aep_channel *ch,
 		}
 	}
 
-	*v2 = correct(ch->aep->aep_context->no_correction, *v1, *v2, ch->map_table->map, ch->map_table->len) / ch->rshunt;
+	*v2 = correct(ch->aep->aep_context->no_correction, *v1, *v2, ch->map_table) / ch->rshunt;
 
 	pwr = *v1 * *v2;
 	if (*v1 > ch->max[0])
@@ -138,8 +138,7 @@ unripe:
 						ADC_COUNTS_PER_VOLT_CH2) + vo1;
 		if ((!(ch->samples & 0x1ff)) && aep->aep_context->show_raw)
 			fprintf(stderr, "%fmV raw shunt;  corr=%fmV\n", v2,
-				correct(aep->aep_context->no_correction, v1, v2, ch->map_table->map,
-							ch->map_table->len));
+				correct(aep->aep_context->no_correction, v1, v2, ch->map_table));
 
 
 		track_limits_convert_to_current(ch, &v1, &v2);
@@ -212,7 +211,7 @@ unripe:
 
 	if ((!(ch->samples & 0x1ff)) && aep->aep_context->show_raw)
 		fprintf(stderr, "%fmV raw shunt;  corr=%fmV\n", v2,
-		      correct(aep->aep_context->no_correction, v1, v2, ch->map_table->map, ch->map_table->len));
+		      correct(aep->aep_context->no_correction, v1, v2, ch->map_table));
 
 	track_limits_convert_to_current(ch, &v1, &v2);
 
@@ -1078,6 +1077,8 @@ post_start:
 	if (fd_with_rx < 0)
 		goto service;
 
+	gettimeofday(&tv, NULL);
+
 	/* somebody had something for us */
 
 	for (m = 0; m <= aep_context->highest; m++) {