capacitor-motioncal 0.0.1

package/common/quality.c

@@ -0,0 +1,257 @@
+#include "imuread.h"
+
+// Discussion of what these 4 quality metrics really do
+// https://forum.pjrc.com/threads/59277-Motion-Sensor-Calibration-Tool-Parameter-Understanding
+
+//static int countdown=1000;
+//static int pr=0;
+
+// return 0 to 99 - which region on the sphere (100 of equal surface area)
+static int sphere_region(float x, float y, float z)
+{
+	float latitude, longitude;
+	int region;
+
+	//if (pr) printf("  region %.1f,%.1f,%.1f  ", x, y, z);
+
+	// longitude = 0 to 2pi  (meaning 0 to 360 degrees)
+	longitude = atan2f(y, x) + (float)M_PI;
+	// latitude = -pi/2 to +pi/2  (meaning -90 to +90 degrees)
+	latitude = (float)(M_PI / 2.0) - atan2f(sqrtf(x * x + y * y), z);
+
+	//if (pr) printf("   lat=%.1f", latitude * (float)(180.0 / M_PI));
+	//if (pr) printf(",lon=%.1f  ", longitude * (float)(180.0 / M_PI));
+
+	// https://etna.mcs.kent.edu/vol.25.2006/pp309-327.dir/pp309-327.html
+	// sphere equations....
+	//  area of unit sphere = 4*pi
+	//  area of unit sphere cap = 2*pi*h  h = cap height
+	//  lattitude of unit sphere cap = arcsin(1 - h)
+	if (latitude > 1.37046f /* 78.52 deg */) {
+		// arctic cap, 1 region
+		region = 0;
+	} else if (latitude < -1.37046f /* -78.52 deg */) {
+		// antarctic cap, 1 region
+		region = 99;
+	} else if (latitude > 0.74776f /* 42.84 deg */ || latitude < -0.74776f ) {
+		// temperate zones, 15 regions each
+		region = floorf(longitude * (float)(15.0 / (M_PI * 2.0)));
+		if (region < 0) region = 0;
+		else if (region > 14) region = 14;
+		if (latitude > 0.0) {
+			region += 1; // 1 to 15
+		} else {
+			region += 84; // 84 to 98
+		}
+	} else {
+		// tropic zones, 34 regions each
+		region = floorf(longitude * (float)(34.0 / (M_PI * 2.0)));
+		if (region < 0) region = 0;
+		else if (region > 33) region = 33;
+		if (latitude >= 0.0) {
+			region += 16; // 16 to 49
+		} else {
+			region += 50; // 50 to 83
+		}
+	}
+	//if (pr) printf("  %d\n", region);
+	return region;
+}
+
+
+static int count=0;
+static int spheredist[100];
+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)
+{
+	float longitude;
+	int i;
+/*
+	countdown--;
+	if (countdown == 0) {
+		countdown = 1000;
+		pr = 1;
+	} else {
+		pr = 0;
+	}
+*/
+	count=0;
+	memset(spheredist, 0, sizeof(spheredist));
+	memset(spheredata, 0, sizeof(spheredata));
+	if (!sphereideal_initialized) {
+		sphereideal[0].x = 0.0f;
+		sphereideal[0].y = 0.0f;
+		sphereideal[0].z = 1.0f;
+		for (i=1; i <= 15; i++) {
+			longitude = ((float)(i - 1) + 0.5f) * (M_PI * 2.0 / 15.0);
+			sphereideal[i].x = cosf(longitude) * cosf(1.05911) * -1.0f;
+			sphereideal[i].y = sinf(longitude) * cosf(1.05911) * -1.0f;
+			sphereideal[i].z = sinf(1.05911);
+		}
+		for (i=16; i <= 49; i++) {
+			longitude = ((float)(i - 16) + 0.5f) * (M_PI * 2.0 / 34.0);
+			sphereideal[i].x = cosf(longitude) * cos(0.37388) * -1.0f;
+			sphereideal[i].y = sinf(longitude) * cos(0.37388) * -1.0f;
+			sphereideal[i].z = sinf(0.37388);
+		}
+		for (i=50; i <= 83; i++) {
+			longitude = ((float)(i - 50) + 0.5f) * (M_PI * 2.0 / 34.0);
+			sphereideal[i].x = cosf(longitude) * cos(0.37388) * -1.0f;
+			sphereideal[i].y = sinf(longitude) * cos(0.37388) * -1.0f;
+			sphereideal[i].z = sinf(-0.37388);
+		}
+		for (i=84; i <= 98; i++) {
+			longitude = ((float)(i - 1) + 0.5f) * (M_PI * 2.0 / 15.0);
+			sphereideal[i].x = cosf(longitude) * cosf(1.05911) * -1.0f;
+			sphereideal[i].y = sinf(longitude) * cosf(1.05911) * -1.0f;
+			sphereideal[i].z = sinf(-1.05911);
+		}
+		sphereideal[99].x = 0.0f;
+		sphereideal[99].y = 0.0f;
+		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)
+{
+	float x, y, z;
+	int region;
+
+	x = point->x;
+	y = point->y;
+	z = point->z;
+	magnitude[count] = sqrtf(x * x + y * y + z * z);
+	region = sphere_region(x, y, z);
+	spheredist[region]++;
+	spheredata[region].x += x;
+	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
+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) {
+			error += 1.0f;
+		} else if (num == 1) {
+			error += 0.2f;
+		} else if (num == 2) {
+			error += 0.01f;
+		}
+	}
+	quality_gaps_buffer = error;
+	quality_gaps_computed = 1;
+	return quality_gaps_buffer;
+}
+
+// Variance in magnitude
+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];
+	}
+	mean = sum / (float)count;
+	variance = 0.0f;
+	for (i=0; i < count; i++) {
+		diff = magnitude[i] - mean;
+		variance += diff * diff;
+	}
+	variance /= (float)count;
+	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
+float quality_wobble_error(void)
+{
+	float sum, radius, x, y, z, xi, yi, zi;
+	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];
+	}
+	radius = sum / (float)count;
+	//if (pr) printf("  radius = %.2f\n", radius);
+	for (i=0; i < 100; i++) {
+		if (spheredist[i] > 0) {
+			//if (pr) printf("  i=%3d", i);
+			x = spheredata[i].x / (float)spheredist[i];
+			y = spheredata[i].y / (float)spheredist[i];
+			z = spheredata[i].z / (float)spheredist[i];
+			//if (pr) printf("  at: %5.1f %5.1f %5.1f :", x, y, z);
+			xi = sphereideal[i].x * radius;
+			yi = sphereideal[i].y * radius;
+			zi = sphereideal[i].z * radius;
+			//if (pr) printf("   ideal: %5.1f %5.1f %5.1f :", xi, yi, zi);
+			xoff += x - xi;
+			yoff += y - yi;
+			zoff += z - zi;
+			//if (pr) printf("\n");
+			n++;
+		}
+	}
+	if (n == 0) return 100.0f;
+	//if (pr) printf("  off = %.2f, %.2f, %.2f\n", xoff, yoff, zoff);
+	xoff /= (float)n;
+	yoff /= (float)n;
+	zoff /= (float)n;
+	//if (pr) printf("  off = %.2f, %.2f, %.2f\n", xoff, yoff, zoff);
+	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
+float quality_spherical_fit_error(void)
+{
+	return magcal.FitError;
+}
+
+short is_send_cal_available(void) {
+	float gaps, variance, wobble, fiterror;
+	
+	gaps = quality_surface_gap_error();
+	variance = quality_magnitude_variance_error();
+	wobble = quality_wobble_error();
+	fiterror = quality_spherical_fit_error();
+	if (gaps < 15.0f && variance < 4.5f && wobble < 4.0f && fiterror < 5.0f) {
+		return 1;
+	} else if (gaps > 20.0f && variance > 5.0f && wobble > 5.0f && fiterror > 6.0f) {
+		return 0;
+	} else {
+		return 0;
+	}
+}

package/common/rawdata.c

@@ -0,0 +1,349 @@
+#include "imuread.h"
+
+extern float draw_points[MAGBUFFSIZE * 3];
+
+static int rawcount=OVERSAMPLE_RATIO;
+static AccelSensor_t accel;
+static MagSensor_t   mag;
+static GyroSensor_t  gyro;
+
+static float cal_data_sent[19];
+static int cal_confirm_needed=0;
+
+void raw_data_reset(void)
+{
+	rawcount = OVERSAMPLE_RATIO;
+	fusion_init();
+	memset(&magcal, 0, sizeof(magcal));
+	magcal.V[2] = 80.0f;  // initial guess
+	magcal.invW[0][0] = 1.0f;
+	magcal.invW[1][1] = 1.0f;
+	magcal.invW[2][2] = 1.0f;
+	magcal.FitError = 100.0f;
+	magcal.FitErrorAge = 100.0f;
+	magcal.B = 50.0f;
+    
+    // Clear draw points
+    clear_draw_points();
+}
+
+static int choose_discard_magcal(void)
+{
+	int32_t rawx, rawy, rawz;
+	int32_t dx, dy, dz;
+	float x, y, z;
+	uint64_t distsq, minsum=0xFFFFFFFFFFFFFFFFull;
+	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)
+{
+	int i;
+
+	// first look for an unused caldata slot
+	for (i=0; i < MAGBUFFSIZE; i++) {
+		if (!magcal.valid[i]) break;
+	}
+	// 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) {
+		i = choose_discard_magcal();
+		if (i < 0 || i >= MAGBUFFSIZE) {
+			i = random() % MAGBUFFSIZE;
+		}
+	}
+	// add it to the cal buffer
+	magcal.BpFast[0][i] = data[6];
+	magcal.BpFast[1][i] = data[7];
+	magcal.BpFast[2][i] = data[8];
+	magcal.valid[i] = 1;
+}
+
+static int is_float_ok(float actual, float expected)
+{
+	float err, maxerr;
+
+	err = fabsf(actual - expected);
+	maxerr = 0.0001f + fabsf(expected) * 0.00003f;
+	if (err <= maxerr) return 1;
+	return 0;
+}
+
+void cal1_data(const float *data)
+{
+	int i, ok;
+
+	if (cal_confirm_needed) {
+		ok = 1;
+		for (i=0; i<10; i++) {
+			if (!is_float_ok(data[i], cal_data_sent[i])) ok = 0;
+		}
+		if (ok) {
+			cal_confirm_needed &= ~1; // got cal1 confirm
+			if (cal_confirm_needed == 0) {
+                // TODO: remove this
+//				calibration_confirmed();
+			}
+		}
+	}
+}
+
+void cal2_data(const float *data)
+{
+	int i, ok;
+
+	if (cal_confirm_needed) {
+		ok = 1;
+		for (i=0; i<9; i++) {
+			if (!is_float_ok(data[i], cal_data_sent[i+10])) ok = 0;
+		}
+		if (ok) {
+			cal_confirm_needed &= ~2; // got cal2 confirm
+			if (cal_confirm_needed == 0) {
+                // TODO: remove this
+//				calibration_confirmed();
+			}
+		}
+	}
+}
+
+void raw_data(const int16_t *data)
+{
+	static int force_orientation_counter=0;
+	float x, y, z, ratio, magdiff;
+	Point_t point;
+
+	add_magcal_data(data);
+	x = magcal.V[0];
+	y = magcal.V[1];
+	z = magcal.V[2];
+	if (MagCal_Run()) {
+		x -= magcal.V[0];
+		y -= magcal.V[1];
+		z -= magcal.V[2];
+		magdiff = sqrtf(x * x + y * y + z * z);
+		//printf("magdiff = %.2f\n", magdiff);
+		if (magdiff > 0.8f) {
+			fusion_init();
+			rawcount = OVERSAMPLE_RATIO;
+			force_orientation_counter = 240;
+		}
+	}
+
+	if (force_orientation_counter > 0) {
+		if (--force_orientation_counter == 0) {
+			//printf("delayed forcible orientation reset\n");
+			fusion_init();
+			rawcount = OVERSAMPLE_RATIO;
+		}
+	}
+
+	if (rawcount >= OVERSAMPLE_RATIO) {
+		memset(&accel, 0, sizeof(accel));
+		memset(&mag, 0, sizeof(mag));
+		memset(&gyro, 0, sizeof(gyro));
+		rawcount = 0;
+	}
+	x = (float)data[0] * G_PER_COUNT;
+	y = (float)data[1] * G_PER_COUNT;
+	z = (float)data[2] * G_PER_COUNT;
+	accel.GpFast[0] = x;
+	accel.GpFast[1] = y;
+	accel.GpFast[2] = z;
+	accel.Gp[0] += x;
+	accel.Gp[1] += y;
+	accel.Gp[2] += z;
+
+	x = (float)data[3] * DEG_PER_SEC_PER_COUNT;
+	y = (float)data[4] * DEG_PER_SEC_PER_COUNT;
+	z = (float)data[5] * DEG_PER_SEC_PER_COUNT;
+	gyro.Yp[0] += x;
+	gyro.Yp[1] += y;
+	gyro.Yp[2] += z;
+	gyro.YpFast[rawcount][0] = x;
+	gyro.YpFast[rawcount][1] = y;
+	gyro.YpFast[rawcount][2] = z;
+
+	apply_calibration(data[6], data[7], data[8], &point);
+	mag.BcFast[0] = point.x;
+	mag.BcFast[1] = point.y;
+	mag.BcFast[2] = point.z;
+	mag.Bc[0] += point.x;
+	mag.Bc[1] += point.y;
+	mag.Bc[2] += point.z;
+
+	rawcount++;
+	if (rawcount >= OVERSAMPLE_RATIO) {
+		ratio = 1.0f / (float)OVERSAMPLE_RATIO;
+		accel.Gp[0] *= ratio;
+		accel.Gp[1] *= ratio;
+		accel.Gp[2] *= ratio;
+		gyro.Yp[0] *= ratio;
+		gyro.Yp[1] *= ratio;
+		gyro.Yp[2] *= ratio;
+		mag.Bc[0] *= ratio;
+		mag.Bc[1] *= ratio;
+		mag.Bc[2] *= ratio;
+		fusion_update(&accel, &mag, &gyro, &magcal);
+		fusion_read(&current_orientation);
+	}
+}
+
+static uint16_t crc16(uint16_t crc, uint8_t data)
+{
+        unsigned int i;
+
+        crc ^= data;
+        for (i = 0; i < 8; ++i) {
+                if (crc & 1) {
+                        crc = (crc >> 1) ^ 0xA001;
+                } else {
+                        crc = (crc >> 1);
+                }
+        }
+        return crc;
+}
+
+static uint8_t * copy_lsb_first(uint8_t *dst, float f)
+{
+	union {
+		float f;
+		uint32_t n;
+	} data;
+
+	data.f = f;
+	*dst++ = data.n;
+	*dst++ = data.n >> 8;
+	*dst++ = data.n >> 16;
+	*dst++ = data.n >> 24;
+	return dst;
+}
+
+static uint8_t data_result[68];
+int send_calibration(void)
+{
+	uint8_t *p, buf[68];
+	uint16_t crc;
+	int i;
+
+	p = buf;
+	*p++ = 117; // 2 byte signature
+	*p++ = 84;
+	for (i=0; i < 3; i++) {
+		p = copy_lsb_first(p, 0.0f); // accelerometer offsets
+		cal_data_sent[0+i] = 0.0f;
+	}
+	for (i=0; i < 3; i++) {
+		p = copy_lsb_first(p, 0.0f); // gyroscope offsets
+		cal_data_sent[3+i] = 0.0f;
+	}
+	for (i=0; i < 3; i++) {
+		p = copy_lsb_first(p, magcal.V[i]); // 12 bytes offset/hardiron
+		cal_data_sent[6+i] = magcal.V[i];
+	}
+	p = copy_lsb_first(p, magcal.B); // field strength
+	p = copy_lsb_first(p, magcal.invW[0][0]); //10
+	p = copy_lsb_first(p, magcal.invW[1][1]); //11
+	p = copy_lsb_first(p, magcal.invW[2][2]); //12
+	p = copy_lsb_first(p, magcal.invW[0][1]); //13
+	p = copy_lsb_first(p, magcal.invW[0][2]); //14
+	p = copy_lsb_first(p, magcal.invW[1][2]); //15
+	cal_data_sent[9] = magcal.B;
+	cal_data_sent[10] = magcal.invW[0][0];
+	cal_data_sent[11] = magcal.invW[0][1];
+	cal_data_sent[12] = magcal.invW[0][2];
+	cal_data_sent[13] = magcal.invW[1][0];
+	cal_data_sent[14] = magcal.invW[1][1];
+	cal_data_sent[15] = magcal.invW[1][2];
+	cal_data_sent[16] = magcal.invW[2][0];
+	cal_data_sent[17] = magcal.invW[2][1];
+	cal_data_sent[18] = magcal.invW[2][2];
+	// PhiEs: update on 17/02/2025 at
+	// Change cal_confirm_needed = 2 to adjust data resend from 
+	// cal_confirm_needed = 3;
+	cal_confirm_needed = 3;
+	// PhiEs: end
+	crc = 0xFFFF;
+	for (i=0; i < 66; i++) {
+		crc = crc16(crc, buf[i]);
+	}
+	*p++ = crc;   // 2 byte crc check
+	*p++ = crc >> 8;
+
+	memcpy(data_result, buf, 68);
+
+	return write_ipc_file_data(buf, 68);
+}
+
+const uint8_t* get_calibration_data(void)
+{
+    return data_result;
+}