Reduce variance when gaps error < 25%

quality.c

@@ -63,6 +63,12 @@ static Point_t spheredata[100];
 static Point_t sphereideal[100];
 static int sphereideal_initialized=0;
 static float magnitude[MAGBUFFSIZE];
+static float quality_gaps_buffer;
+static float quality_variance_buffer;
+static float quality_wobble_buffer;
+static int quality_gaps_computed=0;
+static int quality_variance_computed=0;
+static int quality_wobble_computed=0;
 
 void quality_reset(void)
 {
@@ -113,6 +119,9 @@ void quality_reset(void)
 		sphereideal[99].z = -1.0f;
 		sphereideal_initialized = 1;
 	}
+	quality_gaps_computed = 0;
+	quality_variance_computed = 0;
+	quality_wobble_computed = 0;
 }
 
 void quality_update(const Point_t *point)
@@ -130,6 +139,9 @@ void quality_update(const Point_t *point)
 	spheredata[region].y += y;
 	spheredata[region].z += z;
 	count++;
+	quality_gaps_computed = 0;
+	quality_variance_computed = 0;
+	quality_wobble_computed = 0;
 }
 
 // How many surface gaps
@@ -138,6 +150,7 @@ float quality_surface_gap_error(void)
 	float error=0.0f;
 	int i, num;
 
+	if (quality_gaps_computed) return quality_gaps_buffer;
 	for (i=0; i < 100; i++) {
 		num = spheredist[i];
 		if (num == 0) {
@@ -148,7 +161,9 @@ float quality_surface_gap_error(void)
 			error += 0.01f;
 		}
 	}
-	return error;
+	quality_gaps_buffer = error;
+	quality_gaps_computed = 1;
+	return quality_gaps_buffer;
 }
 
 // Variance in magnitude
@@ -157,6 +172,7 @@ float quality_magnitude_variance_error(void)
 	float sum, mean, diff, variance;
 	int i;
 
+	if (quality_variance_computed) return quality_variance_buffer;
 	sum = 0.0f;
 	for (i=0; i < count; i++) {
 		sum += magnitude[i];
@@ -168,7 +184,9 @@ float quality_magnitude_variance_error(void)
 		variance += diff * diff;
 	}
 	variance /= (float)count;
-	return sqrtf(variance) / mean * 100.0f;
+	quality_variance_buffer = sqrtf(variance) / mean * 100.0f;
+	quality_variance_computed = 1;
+	return quality_variance_buffer;
 }
 
 // Offset of piecewise average data from ideal sphere surface
@@ -178,6 +196,7 @@ float quality_wobble_error(void)
 	float xoff=0.0f, yoff=0.0f, zoff=0.0f;
 	int i, n=0;
 
+	if (quality_wobble_computed) return quality_wobble_buffer;
 	sum = 0.0f;
 	for (i=0; i < count; i++) {
 		sum += magnitude[i];
@@ -208,7 +227,9 @@ float quality_wobble_error(void)
 	yoff /= (float)n;
 	zoff /= (float)n;
 	//if (pr) printf("  off = %.2f, %.2f, %.2f\n", xoff, yoff, zoff);
-	return sqrtf(xoff * xoff + yoff * yoff + zoff * zoff) / radius * 100.0f;
+	quality_wobble_buffer = sqrtf(xoff * xoff + yoff * yoff + zoff * zoff) / radius * 100.0f;
+	quality_wobble_computed = 1;
+	return quality_wobble_buffer;
 }
 
 // Freescale's algorithm fit error

rawdata.c

@@ -20,35 +20,98 @@ void raw_data_reset(void)
 	magcal.B = 50.0f;
 }
 
+static int choose_discard_magcal(void)
+{
+	int32_t rawx, rawy, rawz;
+	int32_t dx, dy, dz;
+	float x, y, z;
+	uint64_t distsq, minsum=0xFFFFFFFFFFFFFFFF;
+	static int runcount=0;
+	int i, j, minindex=0;
+	Point_t point;
+	float gaps, field, error, errormax;
+
+	// When enough data is collected (gaps error is low), assume we
+	// have a pretty good coverage and the field stregth is known.
+	gaps = quality_surface_gap_error();
+	if (gaps < 25.0f) {
+		// occasionally look for points farthest from average field strength
+		// always rate limit assumption-based data purging, but allow the
+		// rate to increase as the angular coverage improves.
+		if (gaps < 1.0f) gaps = 1.0f;
+		if (++runcount > (int)(gaps * 10.0f)) {
+			j = MAGBUFFSIZE;
+			errormax = 0.0f;
+			for (i=0; i < MAGBUFFSIZE; i++) {
+				rawx = magcal.BpFast[0][i];
+				rawy = magcal.BpFast[1][i];
+				rawz = magcal.BpFast[2][i];
+				apply_calibration(rawx, rawy, rawz, &point);
+				x = point.x;
+				y = point.y;
+				z = point.z;
+				field = sqrtf(x * x + y * y + z * z);
+				// if magcal.B is bad, things could go horribly wrong
+				error = fabsf(field - magcal.B);
+				if (error > errormax) {
+					errormax = error;
+					j = i;
+				}
+			}
+			runcount = 0;
+			if (j < MAGBUFFSIZE) {
+				//printf("worst error at %d\n", j);
+				return j;
+			}
+		}
+	} else {
+		runcount = 0;
+	}
+	// When solid info isn't availabe, find 2 points closest to each other,
+	// and randomly discard one.  When we don't have good coverage, this
+	// approach tends to add points into previously unmeasured areas while
+	// discarding info from areas with highly redundant info.
+	for (i=0; i < MAGBUFFSIZE; i++) {
+		for (j=i+1; j < MAGBUFFSIZE; j++) {
+			dx = magcal.BpFast[0][i] - magcal.BpFast[0][j];
+			dy = magcal.BpFast[1][i] - magcal.BpFast[1][j];
+			dz = magcal.BpFast[2][i] - magcal.BpFast[2][j];
+			distsq = (int64_t)dx * (int64_t)dx;
+			distsq += (int64_t)dy * (int64_t)dy;
+			distsq += (int64_t)dz * (int64_t)dz;
+			if (distsq < minsum) {
+				minsum = distsq;
+				minindex = (random() & 1) ? i : j;
+			}
+		}
+	}
+	return minindex;
+}
+
+
 static void add_magcal_data(const int16_t *data)
 {
-	int32_t dx, dy, dz;
-	uint64_t distsq, minsum=0xFFFFFFFFFFFFFFFF;
-	int i, j, minindex=0;
+	int i;
 
 	// first look for an unused caldata slot
 	for (i=0; i < MAGBUFFSIZE; i++) {
 		if (!magcal.valid[i]) break;
 	}
-	// if no unused, find the ones closest to each other
-	// TODO: after reasonable sphere fit, we should retire older data
-	// and choose the ones farthest from the sphere's radius
+	// If the buffer is full, we must choose which old data to discard.
+	// We must choose wisely!  Throwing away the wrong data could prevent
+	// collecting enough data distributed across the entire 3D angular
+	// range, preventing a decent cal from ever happening at all.  Making
+	// any assumption about good vs bad data is particularly risky,
+	// because being wrong could cause an unstable feedback loop where
+	// bad data leads to wrong decisions which leads to even worse data.
+	// But if done well, purging bad data has massive potential to
+	// improve results.  The trick is telling the good from the bad while
+	// still in the process of learning what's good...
 	if (i >= MAGBUFFSIZE) {
-		for (i=0; i < MAGBUFFSIZE; i++) {
-			for (j=i+1; j < MAGBUFFSIZE; j++) {
-				dx = magcal.BpFast[0][i] - magcal.BpFast[0][j];
-				dy = magcal.BpFast[1][i] - magcal.BpFast[1][j];
-				dz = magcal.BpFast[2][i] - magcal.BpFast[2][j];
-				distsq = (int64_t)dx * (int64_t)dx;
-				distsq += (int64_t)dy * (int64_t)dy;
-				distsq += (int64_t)dz * (int64_t)dz;
-				if (distsq < minsum) {
-					minsum = distsq;
-					minindex = (random() & 1) ? i : j;
-				}
-			}
+		i = choose_discard_magcal();
+		if (i < 0 || i >= MAGBUFFSIZE) {
+			i = random() % MAGBUFFSIZE;
 		}
-		i = minindex;
 	}
 	// add it to the cal buffer
 	magcal.BpFast[0][i] = data[6];