jlpoole/MotionCal
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Works with T-Beam Exercise 25_motioncal

Browse source
This commit is contained in:
jlpoole 2026-04-26 08:41:59 -07:00
commit 9c410dae0c
7 changed files with 744 additions and 54 deletions
Download patch file Download diff file Expand all files Collapse all files

61
rawdata.c
View file

@ -1,4 +1,5 @@
#include "imuread.h"
#include <time.h>
static int rawcount=OVERSAMPLE_RATIO;
@ -351,3 +352,63 @@ int send_calibration(void)
return write_serial_data(buf, 68);
}
int save_calibration(char *filename, size_t filename_size)
{
FILE *fp;
time_t now;
struct tm *tm;
int i, j, valid_count=0;
if (filename == NULL || filename_size == 0) return 0;
now = time(NULL);
tm = localtime(&now);
if (tm == NULL) return 0;
if (strftime(filename, filename_size, "MotionCal_settings_%Y%m%d_%H%M%S.txt", tm) == 0) return 0;
fp = fopen(filename, "w");
if (fp == NULL) return 0;
for (i=0; i < MAGBUFFSIZE; i++) {
if (magcal.valid[i]) valid_count++;
}
fprintf(fp, "MotionCal magnetic calibration settings\n");
fprintf(fp, "saved_local_time=%04d-%02d-%02d %02d:%02d:%02d\n",
tm->tm_year + 1900, tm->tm_mon + 1, tm->tm_mday,
tm->tm_hour, tm->tm_min, tm->tm_sec);
fprintf(fp, "valid_points=%d\n", valid_count);
fprintf(fp, "fit_error_percent=%.6f\n", quality_spherical_fit_error());
fprintf(fp, "surface_gap_error_percent=%.6f\n", quality_surface_gap_error());
fprintf(fp, "magnitude_variance_error_percent=%.6f\n", quality_magnitude_variance_error());
fprintf(fp, "wobble_error_percent=%.6f\n", quality_wobble_error());
fprintf(fp, "\n");
fprintf(fp, "magnetic_offset_uT=%.9g,%.9g,%.9g\n",
magcal.V[0], magcal.V[1], magcal.V[2]);
fprintf(fp, "magnetic_field_uT=%.9g\n", magcal.B);
fprintf(fp, "magnetic_mapping_matrix=\n");
for (i=0; i < 3; i++) {
fprintf(fp, " %.9g %.9g %.9g\n",
magcal.invW[i][0], magcal.invW[i][1], magcal.invW[i][2]);
}
fprintf(fp, "\n");
fprintf(fp, "cal1_echo_line=Cal1:0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,%.6f,%.6f,%.6f,%.6f\n",
magcal.V[0], magcal.V[1], magcal.V[2], magcal.B);
fprintf(fp, "cal2_echo_line=Cal2:%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f\n",
magcal.invW[0][0], magcal.invW[0][1], magcal.invW[0][2],
magcal.invW[1][0], magcal.invW[1][1], magcal.invW[1][2],
magcal.invW[2][0], magcal.invW[2][1], magcal.invW[2][2]);
fprintf(fp, "\n");
fprintf(fp, "raw_points=count,x,y,z\n");
for (i=0, j=0; i < MAGBUFFSIZE; i++) {
if (magcal.valid[i]) {
fprintf(fp, "%d,%d,%d,%d\n", j++,
magcal.BpFast[0][i], magcal.BpFast[1][i], magcal.BpFast[2][i]);
}
}
if (fclose(fp) != 0) return 0;
return 1;
}