From b11220a178d3af572df20e87f2bab35cac77ba24 Mon Sep 17 00:00:00 2001 From: PaulStoffregen Date: Fri, 25 Mar 2016 17:50:07 -0700 Subject: [PATCH] Use new calibration data format --- imuread.h | 2 + rawdata.c | 49 +++++++++++--- serialdata.c | 182 ++++++++++++++++++++++++++++++++++++++++++--------- 3 files changed, 193 insertions(+), 40 deletions(-) diff --git a/imuread.h b/imuread.h index 1791f5b..6bcae6f 100644 --- a/imuread.h +++ b/imuread.h @@ -67,6 +67,8 @@ 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 cal1_data(const float *data); +void cal2_data(const float *data); void raw_data(const int16_t *data); int send_calibration(void); void visualize_init(void); diff --git a/rawdata.c b/rawdata.c index 1105f8a..894d64a 100644 --- a/rawdata.c +++ b/rawdata.c @@ -17,6 +17,7 @@ void raw_data_reset(void) magcal.invW[2][2] = 1.0f; magcal.FitError = 100.0f; magcal.FitErrorAge = 100.0f; + magcal.B = 50.0f; } static void add_magcal_data(const int16_t *data) @@ -56,6 +57,29 @@ static void add_magcal_data(const int16_t *data) magcal.valid[i] = 1; } +void cal1_data(const float *data) +{ +#if 0 + int i; + + printf("got cal1_data:\n"); + for (i=0; i<10; i++) { + printf(" %.5f\n", data[i]); + } +#endif +} + +void cal2_data(const float *data) +{ +#if 0 + int i; + + printf("got cal2_data:\n"); + for (i=0; i<9; i++) { + printf(" %.5f\n", data[i]); + } +#endif +} void raw_data(const int16_t *data) { @@ -171,26 +195,35 @@ static uint8_t * copy_lsb_first(uint8_t *dst, float f) int send_calibration(void) { - uint8_t *p, buf[52]; + uint8_t *p, buf[68]; uint16_t crc; - int i, j; + int i; p = buf; *p++ = 117; // 2 byte signature *p++ = 84; for (i=0; i < 3; i++) { - p = copy_lsb_first(p, magcal.V[i]); // 12 bytes offset/hardiron + p = copy_lsb_first(p, 0.0f); // accelerometer offsets } for (i=0; i < 3; i++) { - for (j=0; j < 3; j++) { - p = copy_lsb_first(p, magcal.invW[i][j]); // 36 bytes softiron - } + p = copy_lsb_first(p, 0.0f); // gyroscope offsets } + for (i=0; i < 3; i++) { + p = copy_lsb_first(p, magcal.V[i]); // 12 bytes offset/hardiron + } + 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 crc = 0xFFFF; - for (i=0; i < 50; i++) { + for (i=0; i < 66; i++) { crc = crc16(crc, buf[i]); } *p++ = crc; // 2 byte crc check *p++ = crc >> 8; - return write_serial_data(buf, 52); + return write_serial_data(buf, 68); } + diff --git a/serialdata.c b/serialdata.c index 5885403..c1a5645 100644 --- a/serialdata.c +++ b/serialdata.c @@ -177,10 +177,17 @@ static int packet_parse(const unsigned char *data, int 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; @@ -188,42 +195,153 @@ static int ascii_parse(const unsigned char *data, int len) //print_data("ascii_parse", data, len); end = (const char *)(data + len); for (p = (const char *)data ; p < end; p++) { - 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; - } else if (*p == 10) { - } else { - goto fail; + 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;