capacitor-motioncal 0.0.1

package/common/serialdata.c

@@ -0,0 +1,501 @@
+#include "imuread.h"
+
+void print_data(const char *name, const unsigned char *data, int len)
+{
+	int i;
+
+	printf("%s (%2d bytes):", name, len);
+	for (i = 0; i < len; i++)
+	{
+		printf(" %02X", data[i]);
+	}
+	printf("\n");
+}
+
+static int packet_primary_data(const unsigned char *data)
+{
+	current_orientation.q0 = (float)((int16_t)((data[25] << 8) | data[24])) / 30000.0f;
+	current_orientation.q1 = (float)((int16_t)((data[27] << 8) | data[26])) / 30000.0f;
+	current_orientation.q2 = (float)((int16_t)((data[29] << 8) | data[28])) / 30000.0f;
+	current_orientation.q3 = (float)((int16_t)((data[31] << 8) | data[30])) / 30000.0f;
+
+	return 1;
+}
+
+static int packet_magnetic_cal(const unsigned char *data)
+{
+	int16_t id, x, y, z;
+	int n;
+
+	id = (data[7] << 8) | data[6];
+	x = (data[9] << 8) | data[8];
+	y = (data[11] << 8) | data[10];
+	z = (data[13] << 8) | data[12];
+
+	if (id == 1)
+	{
+		magcal.V[0] = (float)x * 0.1f;
+		magcal.V[1] = (float)y * 0.1f;
+		magcal.V[2] = (float)z * 0.1f;
+		return 1;
+	}
+	else if (id == 2)
+	{
+		magcal.invW[0][0] = (float)x * 0.001f;
+		magcal.invW[1][1] = (float)y * 0.001f;
+		magcal.invW[2][2] = (float)z * 0.001f;
+		return 1;
+	}
+	else if (id == 3)
+	{
+		magcal.invW[0][1] = (float)x / 1000.0f;
+		magcal.invW[1][0] = (float)x / 1000.0f; // TODO: check this assignment
+		magcal.invW[0][2] = (float)y / 1000.0f;
+		magcal.invW[1][2] = (float)y / 1000.0f; // TODO: check this assignment
+		magcal.invW[1][2] = (float)z / 1000.0f;
+		magcal.invW[2][1] = (float)z / 1000.0f; // TODO: check this assignment
+		return 1;
+	}
+	else if (id >= 10 && id < MAGBUFFSIZE + 10)
+	{
+		n = id - 10;
+		if (magcal.valid[n] == 0 || x != magcal.BpFast[0][n] || y != magcal.BpFast[1][n] || z != magcal.BpFast[2][n])
+		{
+			magcal.BpFast[0][n] = x;
+			magcal.BpFast[1][n] = y;
+			magcal.BpFast[2][n] = z;
+			magcal.valid[n] = 1;
+		}
+		return 1;
+	}
+	return 0;
+}
+
+static int packet(const unsigned char *data, int len)
+{
+	if (len <= 0)
+		return 0;
+
+	if (data[0] == 1 && len == 34)
+	{
+		return packet_primary_data(data);
+	}
+	else if (data[0] == 6 && len == 14)
+	{
+		return packet_magnetic_cal(data);
+	}
+	return 0;
+}
+
+static int packet_encoded(const unsigned char *data, int len)
+{
+	const unsigned char *p;
+	unsigned char buf[256];
+	int buflen = 0, copylen;
+
+	p = memchr(data, 0x7D, len);
+	if (p == NULL)
+	{
+		return packet(data, len);
+	}
+	else
+	{
+		while (1)
+		{
+			copylen = p - data;
+			if (copylen > 0)
+			{
+				if (buflen + copylen > sizeof(buf))
+					return 0;
+				memcpy(buf + buflen, data, copylen);
+				buflen += copylen;
+				data += copylen;
+				len -= copylen;
+			}
+			if (buflen + 1 > sizeof(buf))
+				return 0;
+			buf[buflen++] = (p[1] == 0x5E) ? 0x7E : 0x7D;
+			data += 2;
+			len -= 2;
+			if (len <= 0)
+				break;
+			p = memchr(data, 0x7D, len);
+			if (p == NULL)
+			{
+				if (buflen + len > sizeof(buf))
+					return 0;
+				memcpy(buf + buflen, data, len);
+				buflen += len;
+				break;
+			}
+		}
+		return packet(buf, buflen);
+	}
+}
+
+static int packet_parse(const unsigned char *data, int len)
+{
+	static unsigned char packetbuf[256];
+	static unsigned int packetlen = 0;
+	const unsigned char *p;
+	int copylen;
+	int ret = 0;
+
+	// print_data("packet_parse", data, len);
+	while (len > 0)
+	{
+		p = memchr(data, 0x7E, len);
+		if (p == NULL)
+		{
+			if (packetlen + len > sizeof(packetbuf))
+			{
+				packetlen = 0;
+				return 0; // would overflow buffer
+			}
+			memcpy(packetbuf + packetlen, data, len);
+			packetlen += len;
+			len = 0;
+		}
+		else if (p > data)
+		{
+			copylen = p - data;
+			if (packetlen + copylen > sizeof(packetbuf))
+			{
+				packetlen = 0;
+				return 0; // would overflow buffer
+			}
+			memcpy(packetbuf + packetlen, data, copylen);
+			packet_encoded(packetbuf, packetlen + copylen);
+			packetlen = 0;
+			data += copylen + 1;
+			len -= copylen + 1;
+		}
+		else
+		{
+			if (packetlen > 0)
+			{
+				if (packet_encoded(packetbuf, packetlen))
+					ret = 1;
+				packetlen = 0;
+			}
+			data++;
+			len--;
+		}
+	}
+	return ret;
+}
+
+#define ASCII_STATE_WORD 0
+#define ASCII_STATE_RAW 1
+#define ASCII_STATE_CAL1 2
+#define ASCII_STATE_CAL2 3
+
+static int ascii_parse(const unsigned char *data, int len)
+{
+	static int ascii_state = ASCII_STATE_WORD;
+	static int ascii_num = 0, ascii_neg = 0, ascii_count = 0;
+	static int16_t ascii_raw_data[9];
+	static float ascii_cal_data[10];
+	static unsigned int ascii_raw_data_count = 0;
+	const char *p, *end;
+	int ret = 0;
+
+	// print_data("ascii_parse", data, len);
+	end = (const char *)(data + len);
+	for (p = (const char *)data; p < end; p++)
+	{
+		if (ascii_state == ASCII_STATE_WORD)
+		{
+			if (ascii_count == 0)
+			{
+				if (*p == 'R')
+				{
+					ascii_num = ASCII_STATE_RAW;
+					ascii_count = 1;
+				}
+				else if (*p == 'C')
+				{
+					ascii_num = ASCII_STATE_CAL1;
+					ascii_count = 1;
+				}
+			}
+			else if (ascii_count == 1)
+			{
+				if (*p == 'a')
+				{
+					ascii_count = 2;
+				}
+				else
+				{
+					ascii_num = 0;
+					ascii_count = 0;
+				}
+			}
+			else if (ascii_count == 2)
+			{
+				if (*p == 'w' && ascii_num == ASCII_STATE_RAW)
+				{
+					ascii_count = 3;
+				}
+				else if (*p == 'l' && ascii_num == ASCII_STATE_CAL1)
+				{
+					ascii_count = 3;
+				}
+				else
+				{
+					ascii_num = 0;
+					ascii_count = 0;
+				}
+			}
+			else if (ascii_count == 3)
+			{
+				if (*p == ':' && ascii_num == ASCII_STATE_RAW)
+				{
+					ascii_state = ASCII_STATE_RAW;
+					ascii_raw_data_count = 0;
+					ascii_num = 0;
+					ascii_count = 0;
+				}
+				else if (*p == '1' && ascii_num == ASCII_STATE_CAL1)
+				{
+					ascii_count = 4;
+				}
+				else if (*p == '2' && ascii_num == ASCII_STATE_CAL1)
+				{
+					ascii_num = ASCII_STATE_CAL2;
+					ascii_count = 4;
+				}
+				else
+				{
+					ascii_num = 0;
+					ascii_count = 0;
+				}
+			}
+			else if (ascii_count == 4)
+			{
+				if (*p == ':' && ascii_num == ASCII_STATE_CAL1)
+				{
+					ascii_state = ASCII_STATE_CAL1;
+					ascii_raw_data_count = 0;
+					ascii_num = 0;
+					ascii_count = 0;
+				}
+				else if (*p == ':' && ascii_num == ASCII_STATE_CAL2)
+				{
+					ascii_state = ASCII_STATE_CAL2;
+					ascii_raw_data_count = 0;
+					ascii_num = 0;
+					ascii_count = 0;
+				}
+				else
+				{
+					ascii_num = 0;
+					ascii_count = 0;
+				}
+			}
+			else
+			{
+				goto fail;
+			}
+		}
+		else if (ascii_state == ASCII_STATE_RAW)
+		{
+			if (*p == '-')
+			{
+				// printf("ascii_parse negative\n");
+				if (ascii_count > 0)
+					goto fail;
+				ascii_neg = 1;
+			}
+			else if (isdigit(*p))
+			{
+				// printf("ascii_parse digit\n");
+				ascii_num = ascii_num * 10 + *p - '0';
+				ascii_count++;
+			}
+			else if (*p == ',')
+			{
+				// printf("ascii_parse comma, %d\n", ascii_num);
+				if (ascii_neg)
+					ascii_num = -ascii_num;
+				if (ascii_num < -32768 && ascii_num > 32767)
+					goto fail;
+				if (ascii_raw_data_count >= 8)
+					goto fail;
+				ascii_raw_data[ascii_raw_data_count++] = ascii_num;
+				ascii_num = 0;
+				ascii_neg = 0;
+				ascii_count = 0;
+			}
+			else if (*p == 13)
+			{
+				// printf("ascii_parse newline\n");
+				if (ascii_neg)
+					ascii_num = -ascii_num;
+				if (ascii_num < -32768 && ascii_num > 32767)
+					goto fail;
+				if (ascii_raw_data_count != 8)
+					goto fail;
+				ascii_raw_data[ascii_raw_data_count] = ascii_num;
+				raw_data(ascii_raw_data);
+				ret = 1;
+				ascii_raw_data_count = 0;
+				ascii_num = 0;
+				ascii_neg = 0;
+				ascii_count = 0;
+				ascii_state = ASCII_STATE_WORD;
+			}
+			else if (*p == 10)
+			{
+			}
+			else
+			{
+				goto fail;
+			}
+		}
+		else if (ascii_state == ASCII_STATE_CAL1 || ascii_state == ASCII_STATE_CAL2)
+		{
+			if (*p == '-')
+			{
+				// printf("ascii_parse negative\n");
+				if (ascii_count > 0)
+					goto fail;
+				ascii_neg = 1;
+			}
+			else if (isdigit(*p))
+			{
+				// printf("ascii_parse digit\n");
+				ascii_num = ascii_num * 10 + *p - '0';
+				ascii_count++;
+			}
+			else if (*p == '.')
+			{
+				// printf("ascii_parse decimal, %d\n", ascii_num);
+				if (ascii_raw_data_count > 9)
+					goto fail;
+				ascii_cal_data[ascii_raw_data_count] = (float)ascii_num;
+				ascii_num = 0;
+				ascii_count = 0;
+			}
+			else if (*p == ',')
+			{
+				// printf("ascii_parse comma, %d\n", ascii_num);
+				if (ascii_raw_data_count > 9)
+					goto fail;
+				ascii_cal_data[ascii_raw_data_count] +=
+					(float)ascii_num / powf(10.0f, ascii_count);
+				if (ascii_neg)
+					ascii_cal_data[ascii_raw_data_count] *= -1.0f;
+				ascii_raw_data_count++;
+				ascii_num = 0;
+				ascii_neg = 0;
+				ascii_count = 0;
+			}
+			else if (*p == 13)
+			{
+				// printf("ascii_parse newline\n");
+				if ((ascii_state == ASCII_STATE_CAL1 && ascii_raw_data_count != 9) || (ascii_state == ASCII_STATE_CAL2 && ascii_raw_data_count != 8))
+					goto fail;
+				ascii_cal_data[ascii_raw_data_count] +=
+					(float)ascii_num / powf(10.0f, ascii_count);
+				if (ascii_neg)
+					ascii_cal_data[ascii_raw_data_count] *= -1.0f;
+				if (ascii_state == ASCII_STATE_CAL1)
+				{
+					cal1_data(ascii_cal_data);
+				}
+				else if (ascii_state == ASCII_STATE_CAL2)
+				{
+					cal2_data(ascii_cal_data);
+				}
+				ret = 1;
+				ascii_raw_data_count = 0;
+				ascii_num = 0;
+				ascii_neg = 0;
+				ascii_count = 0;
+				ascii_state = ASCII_STATE_WORD;
+			}
+			else if (*p == 10)
+			{
+			}
+			else
+			{
+				goto fail;
+			}
+		}
+	}
+	return ret;
+fail:
+	// printf("ascii FAIL\n");
+	ascii_state = ASCII_STATE_WORD;
+	ascii_raw_data_count = 0;
+	ascii_num = 0;
+	ascii_neg = 0;
+	ascii_count = 0;
+	return 0;
+}
+
+static void newdata(const unsigned char *data, int len)
+{
+	packet_parse(data, len);
+	ascii_parse(data, len);
+	// TODO: learn which one and skip the other
+}
+
+// PhiEs start ======= 
+void clear_file(const char *filename) {
+	FILE *file = fopen(filename, "w");
+	if (file != NULL)
+	{
+		fclose(file);
+	}
+}
+
+int read_ipc_file_data(const char *filename)
+{
+	char buf[256];
+	FILE *inFile = fopen(filename, "rb");
+	if(inFile) {
+		if (fgets(buf,sizeof(buf),inFile) != NULL)
+		{
+			clear_file(filename);
+			// Conver to uchar
+			unsigned char uchar_data[256];
+			for (size_t i = 0; i < strlen(buf); i++) {
+				uchar_data[i] = (unsigned char)buf[i];
+			}
+			uchar_data[strlen(buf)] = '\0';
+			newdata(uchar_data, sizeof(uchar_data));
+		}
+		fclose(inFile);
+		return sizeof(buf);
+	}
+	return 0;
+}
+
+static char result_filename[512];
+int write_ipc_file_data(const void *ptr, int len)
+{
+	FILE *file = fopen(result_filename, "wb");
+    if (!file) {
+        perror("Error opening file");
+        return -1;
+    }
+
+    int written = fwrite(ptr, 1, len, file);
+    if (written < len) {
+        perror("Error writing to file");
+        fclose(file);
+        return -1;
+    }
+
+    fclose(file);
+    return written;
+}
+
+// PhiEs end ======= 
+
+void set_result_filename(const char *filename) {
+	strncpy(result_filename, filename, sizeof(result_filename) - 1);
+    result_filename[sizeof(result_filename) - 1] = '\0';
+}

package/common/visualize.c

@@ -0,0 +1,131 @@
+#include "imuread.h"
+
+MagCalibration_t magcal;
+
+Quaternion_t current_orientation;
+
+static float draw_points[MAGBUFFSIZE * 3];  // Each point has x,y,z coordinates
+
+void apply_calibration(int16_t rawx, int16_t rawy, int16_t rawz, Point_t *out)
+{
+    float x, y, z;
+
+    x = ((float)rawx * UT_PER_COUNT) - magcal.V[0];
+    y = ((float)rawy * UT_PER_COUNT) - magcal.V[1];
+    z = ((float)rawz * UT_PER_COUNT) - magcal.V[2];
+    out->x = x * magcal.invW[0][0] + y * magcal.invW[0][1] + z * magcal.invW[0][2];
+    out->y = x * magcal.invW[1][0] + y * magcal.invW[1][1] + z * magcal.invW[1][2];
+    out->z = x * magcal.invW[2][0] + y * magcal.invW[2][1] + z * magcal.invW[2][2];
+}
+
+static void quad_to_rotation(const Quaternion_t *quat, float *rmatrix)
+{
+    float qw = quat->q0;
+    float qx = quat->q1;
+    float qy = quat->q2;
+    float qz = quat->q3;
+    rmatrix[0] = 1.0f - 2.0f * qy * qy - 2.0f * qz * qz;
+    rmatrix[1] = 2.0f * qx * qy - 2.0f * qz * qw;
+    rmatrix[2] = 2.0f * qx * qz + 2.0f * qy * qw;
+    rmatrix[3] = 2.0f * qx * qy + 2.0f * qz * qw;
+    rmatrix[4] = 1.0f  - 2.0f * qx * qx - 2.0f * qz * qz;
+    rmatrix[5] = 2.0f * qy * qz - 2.0f * qx * qw;
+    rmatrix[6] = 2.0f * qx * qz - 2.0f * qy * qw;
+    rmatrix[7] = 2.0f * qy * qz + 2.0f * qx * qw;
+    rmatrix[8] = 1.0f  - 2.0f * qx * qx - 2.0f * qy * qy;
+}
+
+static void rotate(const Point_t *in, Point_t *out, const float *rmatrix)
+{
+    out->x = in->x * rmatrix[0] + in->y * rmatrix[1] + in->z * rmatrix[2];
+    out->y = in->x * rmatrix[3] + in->y * rmatrix[4] + in->z * rmatrix[5];
+    out->z = in->x * rmatrix[6] + in->y * rmatrix[7] + in->z * rmatrix[8];
+}
+
+int invert_q0=0;
+int invert_q1=0;
+int invert_q2=0;
+int invert_q3=1;
+int invert_x=0;
+int invert_y=0;
+int invert_z=0;
+
+void display_callback(void)
+{
+    int i;
+    float xscale, yscale, zscale;
+    float xoff, yoff, zoff;
+    float rotation[9];
+    Point_t point, draw;
+    Quaternion_t orientation;
+
+    quality_reset();
+
+    xscale = 0.05;
+    yscale = 0.05;
+    zscale = 0.05;
+    xoff = 0.0;
+    yoff = 0.0;
+    zoff = -7.0;
+
+    memcpy(&orientation, &current_orientation, sizeof(orientation));
+    if (invert_q0) orientation.q0 *= -1.0f;
+    if (invert_q1) orientation.q1 *= -1.0f;
+    if (invert_q2) orientation.q2 *= -1.0f;
+    if (invert_q3) orientation.q3 *= -1.0f;
+    quad_to_rotation(&orientation, rotation);
+    for (i=0; i < MAGBUFFSIZE; i++) {
+        draw_points[i*3] = 0.0f;      // Fix indexing
+        draw_points[i*3+1] = 0.0f;    // Fix indexing
+        draw_points[i*3+2] = 0.0f;    // Fix indexing
+        if (magcal.valid[i]) {
+            apply_calibration(magcal.BpFast[0][i], magcal.BpFast[1][i], magcal.BpFast[2][i], &point);
+            quality_update(&point);
+            rotate(&point, &draw, rotation);
+            if (invert_x) draw.x *= -1.0f;
+            if (invert_y) draw.y *= -1.0f;
+            if (invert_z) draw.z *= -1.0f;
+            draw_points[i*3] = draw.x * xscale + xoff;
+            draw_points[i*3+1] = draw.z * yscale + yoff;
+            draw_points[i*3+2] = draw.y * zscale + zoff;
+        }
+    }
+}
+
+float* get_draw_points(void)
+{
+    return draw_points;
+}
+
+int get_draw_points_count(void)
+{
+    return MAGBUFFSIZE;
+}
+
+void clear_draw_points(void)
+{
+    memset(draw_points, 0, sizeof(draw_points));
+}
+
+// Implementation of MagCalibration_t property accessor functions
+void get_hard_iron_offset(float V[3])
+{
+    V[0] = magcal.V[0];
+    V[1] = magcal.V[1];
+    V[2] = magcal.V[2];
+}
+
+void get_soft_iron_matrix(float invW[3][3])
+{
+    int i, j;
+    for (i = 0; i < 3; i++) {
+        for (j = 0; j < 3; j++) {
+            invW[i][j] = magcal.invW[i][j];
+        }
+    }
+}
+
+float get_geomagnetic_field_magnitude(void)
+{
+    return magcal.B;
+}