diff --git a/imuread.c b/imuread.c index cb3fea7..83ba30c 100644 --- a/imuread.c +++ b/imuread.c @@ -21,6 +21,23 @@ static void glut_display_callback(void) glutSwapBuffers(); } +static void glut_keystroke_callback(unsigned char ch, int x, int y) +{ + if (magcal.fFitErrorpc > 9.0) { + printf("Poor Calibration: "); + printf("soft iron fit error = %.1f%%\n", magcal.fFitErrorpc); + return; + } + printf("Magnetic Calibration: (%.1f%% fit error)\n", magcal.fFitErrorpc); + printf(" %7.2f %6.3f %6.3f %6.3f\n", + magcal.fV[0], magcal.finvW[0][0], magcal.finvW[0][1], magcal.finvW[0][2]); + printf(" %7.2f %6.3f %6.3f %6.3f\n", + magcal.fV[1], magcal.finvW[1][0], magcal.finvW[1][1], magcal.finvW[1][2]); + printf(" %7.2f %6.3f %6.3f %6.3f\n", + magcal.fV[2], magcal.finvW[2][0], magcal.finvW[2][1], magcal.finvW[2][2]); + send_calibration(); +} + int main(int argc, char *argv[]) { raw_data_reset(); @@ -35,6 +52,7 @@ int main(int argc, char *argv[]) glutReshapeFunc(resize_callback); glutDisplayFunc(glut_display_callback); glutTimerFunc(TIMEOUT_MSEC, timer_callback, 0); + glutKeyboardFunc(glut_keystroke_callback); if (!open_port(PORT)) die("Unable to open %s\n", PORT); glutMainLoop(); diff --git a/imuread.h b/imuread.h index 51b644d..c482d0b 100644 --- a/imuread.h +++ b/imuread.h @@ -62,9 +62,11 @@ extern Quaternion_t current_orientation; extern int open_port(const char *name); extern int read_serial_data(void); +extern int write_serial_data(const void *ptr, int len); extern void close_port(void); void raw_data_reset(void); void raw_data(const int16_t *data); +int send_calibration(void); void visualize_init(void); void apply_calibration(int16_t rawx, int16_t rawy, int16_t rawz, Point_t *out); void display_callback(void); @@ -79,6 +81,7 @@ typedef struct { float fB; // current geomagnetic field magnitude (uT) float fFourBsq; // current 4*B*B (uT^2) float fFitErrorpc; // current fit error % + float fFitErrorAge; // current fit error % (grows automatically with age) float ftrV[3]; // trial value of hard iron offset z, y, z (uT) float ftrinvW[3][3]; // trial inverse soft iron matrix size float ftrB; // trial value of geomagnetic field magnitude in uT diff --git a/magcal.c b/magcal.c index 2f6c5bc..4db357a 100644 --- a/magcal.c +++ b/magcal.c @@ -75,7 +75,7 @@ void MagCal_Run(void) if (magcal.iValidMagCal) { // age the existing fit error to avoid one good calibration locking out future updates - magcal.fFitErrorpc *= 1.02f; + magcal.fFitErrorAge *= 1.01f; } // is enough data collected @@ -97,12 +97,13 @@ void MagCal_Run(void) // 2: the calibration fit is reduced or // 3: an improved solver was used giving a good trial calibration (4% or under) if ((magcal.iValidMagCal == 0) || - (magcal.ftrFitErrorpc <= magcal.fFitErrorpc) || + (magcal.ftrFitErrorpc <= magcal.fFitErrorAge) || ((isolver > magcal.iValidMagCal) && (magcal.ftrFitErrorpc <= 4.0F))) { // accept the new calibration solution //printf("new magnetic cal, B=%.2f uT\n", magcal.ftrB); magcal.iValidMagCal = isolver; magcal.fFitErrorpc = magcal.ftrFitErrorpc; + magcal.fFitErrorAge = magcal.ftrFitErrorpc; magcal.fB = magcal.ftrB; magcal.fFourBsq = 4.0F * magcal.ftrB * magcal.ftrB; for (i = X; i <= Z; i++) { diff --git a/rawdata.c b/rawdata.c index bf2d3c2..8a5e55e 100644 --- a/rawdata.c +++ b/rawdata.c @@ -12,11 +12,12 @@ void raw_data_reset(void) rawcount = OVERSAMPLE_RATIO; fInit_9DOF_GBY_KALMAN(&fusionstate, 100, OVERSAMPLE_RATIO); memset(&magcal, 0, sizeof(magcal)); - magcal.fV[1] = 10.0f; magcal.fV[2] = 80.0f; // initial guess magcal.finvW[0][0] = 1.0f; magcal.finvW[1][1] = 1.0f; magcal.finvW[2][2] = 1.0f; + magcal.fFitErrorpc = 100.0f; + magcal.fFitErrorAge = 100.0f; } static void add_magcal_data(const int16_t *data) @@ -118,5 +119,58 @@ void raw_data(const int16_t *data) } } +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; +} + +int send_calibration(void) +{ + uint8_t *p, buf[52]; + uint16_t crc; + int i, j; + + p = buf; + *p++ = 117; // 2 byte signature + *p++ = 84; + for (i=0; i < 3; i++) { + p = copy_lsb_first(p, magcal.fV[i]); // 12 bytes offset/hardiron + } + for (i=0; i < 3; i++) { + for (j=0; j < 3; j++) { + p = copy_lsb_first(p, magcal.finvW[i][j]); // 36 bytes softiron + } + } + crc = 0xFFFF; + for (i=0; i < 50; i++) { + crc = crc16(crc, buf[i]); + } + *p++ = crc; // 2 byte crc check + *p++ = crc >> 8; + return write_serial_data(buf, 52); +} diff --git a/serialdata.c b/serialdata.c index edbc774..365fec2 100644 --- a/serialdata.c +++ b/serialdata.c @@ -240,6 +240,7 @@ static void newdata(const unsigned char *data, int len) } + #if defined(LINUX) || defined(MACOSX) static int portfd=-1; @@ -300,6 +301,34 @@ int read_serial_data(void) } } +int write_serial_data(const void *ptr, int len) +{ + int n, written=0; + fd_set wfds; + struct timeval tv; + + //printf("Write %d\n", len); + if (portfd < 0) return -1; + while (written < len) { + n = write(portfd, (const char *)ptr + written, len - written); + if (n < 0 && (errno == EAGAIN || errno == EINTR)) n = 0; + //printf("Write, n = %d\n", n); + if (n < 0) return -1; + if (n > 0) { + written += n; + } else { + tv.tv_sec = 0; + tv.tv_usec = 5000; + FD_ZERO(&wfds); + FD_SET(portfd, &wfds); + n = select(portfd+1, NULL, &wfds, NULL, &tv); + if (n < 0 && errno == EINTR) n = 1; + if (n <= 0) return -1; + } + } + return written; +} + void close_port(void) { if (portfd >= 0) { @@ -425,6 +454,37 @@ int read_serial_data(void) return r; } +int write_serial_data(const void *ptr, int len) +{ + DWORD num_written; + OVERLAPPED ov; + int r; + + ov.hEvent = CreateEvent(NULL, TRUE, FALSE, NULL); + if (ov.hEvent == NULL) return -1; + ov.Internal = ov.InternalHigh = 0; + ov.Offset = ov.OffsetHigh = 0; + if (WriteFile(port_handle, ptr, len, &num_written, &ov)) { + //printf("Write, immediate complete, num_written=%lu\n", num_written); + r = num_written; + } else { + if (GetLastError() == ERROR_IO_PENDING) { + if (GetOverlappedResult(port_handle, &ov, &num_written, TRUE)) { + //printf("Write, delayed, num_written=%lu\n", num_written); + r = num_written; + } else { + //printf("Write, delayed error\n"); + r = -1; + } + } else { + //printf("Write, error\n"); + r = -1; + } + }; + CloseHandle(ov.hEvent); + return r; +} + void close_port(void) { CloseHandle(port_handle);