diff --git a/Makefile b/Makefile index ed3760c..7eb78eb 100644 --- a/Makefile +++ b/Makefile @@ -35,7 +35,7 @@ VERSION = 0.01 endif -OBJS = visualize.o serialdata.o rawdata.o magcal.o matrix.o fusion.o +OBJS = visualize.o serialdata.o rawdata.o magcal.o matrix.o fusion.o quality.o all: $(ALL) @@ -71,5 +71,6 @@ rawdata.o: rawdata.c imuread.h magcal.o: magcal.c imuread.h matrix.o: matrix.c imuread.h fusion.o: fusion.c imuread.h +quality.o: quality.c imuread.h diff --git a/imuread.h b/imuread.h index 5c6122a..fe4793f 100644 --- a/imuread.h +++ b/imuread.h @@ -74,7 +74,12 @@ void apply_calibration(int16_t rawx, int16_t rawy, int16_t rawz, Point_t *out); void display_callback(void); void resize_callback(int width, int height); void MagCal_Run(void); - +void quality_reset(void); +void quality_update(const Point_t *point); +float quality_surface_gap_error(void); +float quality_magnitude_variance_error(void); +float quality_centroid_placement_error(void); +float quality_spherical_fit_error(void); // magnetic calibration & buffer structure typedef struct { diff --git a/quality.c b/quality.c new file mode 100644 index 0000000..a6b9d4d --- /dev/null +++ b/quality.c @@ -0,0 +1,122 @@ +#include "imuread.h" + + +// longitude = 0 to 2pi (meaning 0 to 360 degrees) +// latitude = -pi/2 to +pi/2 (meaning -90 to +90 degrees) +// return 0 to 99 - which region on the sphere (100 of equal surface area) +static int sphere_region(float longitude, float latitude) +{ + int region; + + // 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 + return 0; + } else if (latitude < -1.37046f /* -78.52 deg */) { + // antarctic cap, 1 region + return 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) { + return region + 1; // 1 to 15 + } else { + return 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) { + return region + 16; // 16 to 49 + } else { + return region + 50; // 50 to 83 + } + } +} + + +static int count=0; +static int spheredist[100]; +static float magnitude[MAGBUFFSIZE]; + +void quality_reset(void) +{ + count=0; + memset(spheredist, 0, sizeof(spheredist)); +} + +void quality_update(const Point_t *point) +{ + float x, y, z; + float latitude, longitude; + + x = point->x; + y = point->y; + z = point->z; + magnitude[count] = sqrtf(x * x + y * y + z * z); + longitude = atan2f(y, x) + (float)M_PI; + latitude = atan2f(sqrtf(x * x + y * y), z) - (float)(M_PI / 2.0); + spheredist[sphere_region(longitude, latitude)]++; + count++; +} + +// How many surface gaps +float quality_surface_gap_error(void) +{ + float error=0.0f; + int i, num; + + 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; + } + } + return error; +} + +// Variance in magnitude +float quality_magnitude_variance_error(void) +{ + float sum, mean, diff, variance; + int i; + + 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; + return sqrtf(variance) / mean * 100.0f; +} + +// Offset of centroid +float quality_centroid_placement_error(void) +{ + return 0.0f; +} + +// Freescale's algorithm fit error +float quality_spherical_fit_error(void) +{ + return magcal.FitError; +} + + diff --git a/visualize.c b/visualize.c index 6dd7efe..bc58408 100644 --- a/visualize.c +++ b/visualize.c @@ -4,47 +4,6 @@ MagCalibration_t magcal; Quaternion_t current_orientation; -// longitude = 0 to 2pi (meaning 0 to 360 degrees) -// latitude = -pi/2 to +pi/2 (meaning -90 to +90 degrees) -// return 0 to 99 - which region on the sphere (100 of equal surface area) -static int sphere_region(float longitude, float latitude) -{ - int region; - - // 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 - return 0; - } else if (latitude < -1.37046f /* -78.52 deg */) { - // antarctic cap, 1 region - return 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) { - return region + 1; // 1 to 15 - } else { - return 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) { - return region + 16; // 16 to 49 - } else { - return region + 50; // 50 to 83 - } - } -} - void apply_calibration(int16_t rawx, int16_t rawy, int16_t rawz, Point_t *out) { @@ -89,22 +48,16 @@ static GLuint spherelowreslist; void display_callback(void) { - //static int updatenum=0; - int i, count=0; + int i; float xscale, yscale, zscale; float xoff, yoff, zoff; float rotation[9]; Point_t point, draw; - float magnitude[MAGBUFFSIZE]; - float latitude[MAGBUFFSIZE]; // 0 to PI - float longitude[MAGBUFFSIZE]; // -PI to +PI - float sumx=0.0, sumy=0.0, sumz=0.0, summag=0.0; - int spheredist[100]; Quaternion_t orientation; + quality_reset(); glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glColor3f(1, 0, 0); // set current color to red - memset(spheredist, 0, sizeof(spheredist)); glLoadIdentity(); #if 0 gluLookAt(0.0, 0.0, 0.8, // eye location @@ -129,25 +82,13 @@ void display_callback(void) orientation.q3 *= -1.0f; quad_to_rotation(&orientation, rotation); for (i=0; i < MAGBUFFSIZE; i++) { - //if (caldata[i].valid) { if (magcal.valid[i]) { - //apply_calibration(&caldata[i], &point); apply_calibration(magcal.BpFast[0][i], magcal.BpFast[1][i], magcal.BpFast[2][i], &point); //point.x *= -1.0f; //point.y *= -1.0f; //point.z *= -1.0f; - sumx += point.x; - sumy += point.y; - sumz += point.z; - magnitude[i] = sqrtf(point.x * point.x + - point.y * point.y + point.z * point.z); - summag += magnitude[i]; - longitude[i] = atan2f(point.y, point.x) + (float)M_PI; - latitude[i] = atan2f(sqrtf(point.x * point.x + - point.y * point.y), point.z) - (float)(M_PI / 2.0); - spheredist[sphere_region(longitude[i], latitude[i])]++; - count++; + quality_update(&point); rotate(&point, &draw, rotation); glPushMatrix(); glTranslatef( @@ -165,23 +106,10 @@ void display_callback(void) } } #if 0 - if (updatenum++ == 500) { - for (i=0; i < MAGBUFFSIZE; i++) { - if (caldata[i].valid) { - printf("long: %6.2f lat: %6.2f (%.2f)\n", - longitude[i] * 180.0 / M_PI, - latitude[i] * 180.0 / M_PI, latitude[i]); - } - } - for (i=0; i < 100; i++) { - printf("sphere %d region: %d points\n", 1, spheredist[i]); - } - printf("count = %d\n", count); - printf("sum = %.2f %.2f %.2f\n", sumx, sumy, sumz); - printf("summag = %.2f avg: %.2f\n", summag, summag / (float)count); - printf("exit here\n"); - exit(1); - } + printf(" quality: %5.1f %5.1f %5.1f\n", + quality_surface_gap_error(), + quality_magnitude_variance_error(), + quality_spherical_fit_error()); #endif }