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

Use new calibration data format

Browse source
This commit is contained in:
PaulStoffregen 2016-03-25 17:50:07 -07:00
commit b11220a178
3 changed files with 193 additions and 40 deletions
Download patch file Download diff file Expand all files Collapse all files

2
imuread.h
View file

@ -67,6 +67,8 @@ extern int read_serial_data(void);
extern int write_serial_data(const void *ptr, int len); extern int write_serial_data(const void *ptr, int len);
extern void close_port(void); extern void close_port(void);
void raw_data_reset(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); void raw_data(const int16_t *data);
int send_calibration(void); int send_calibration(void);
void visualize_init(void); void visualize_init(void);

49
rawdata.c
View file

@ -17,6 +17,7 @@ void raw_data_reset(void)
magcal.invW[2][2] = 1.0f; magcal.invW[2][2] = 1.0f;
magcal.FitError = 100.0f; magcal.FitError = 100.0f;
magcal.FitErrorAge = 100.0f; magcal.FitErrorAge = 100.0f;
magcal.B = 50.0f;
} }
static void add_magcal_data(const int16_t *data) 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; 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) 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) int send_calibration(void)
{ {
uint8_t *p, buf[52]; uint8_t *p, buf[68];
uint16_t crc; uint16_t crc;
int i, j; int i;
p = buf; p = buf;
*p++ = 117; // 2 byte signature *p++ = 117; // 2 byte signature
*p++ = 84; *p++ = 84;
for (i=0; i < 3; i++) { 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 (i=0; i < 3; i++) {
for (j=0; j < 3; j++) { p = copy_lsb_first(p, 0.0f); // gyroscope offsets
p = copy_lsb_first(p, magcal.invW[i][j]); // 36 bytes softiron
}
} }
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; crc = 0xFFFF;
for (i=0; i < 50; i++) { for (i=0; i < 66; i++) {
crc = crc16(crc, buf[i]); crc = crc16(crc, buf[i]);
} }
*p++ = crc; // 2 byte crc check *p++ = crc; // 2 byte crc check
*p++ = crc >> 8; *p++ = crc >> 8;
return write_serial_data(buf, 52); return write_serial_data(buf, 68);
} }

182
serialdata.c
View file

@ -177,10 +177,17 @@ static int packet_parse(const unsigned char *data, int len)
return ret; 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_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 int ascii_num=0, ascii_neg=0, ascii_count=0;
static int16_t ascii_raw_data[9]; static int16_t ascii_raw_data[9];
static float ascii_cal_data[10];
static unsigned int ascii_raw_data_count=0; static unsigned int ascii_raw_data_count=0;
const char *p, *end; const char *p, *end;
int ret=0; int ret=0;
@ -188,42 +195,153 @@ static int ascii_parse(const unsigned char *data, int len)
//print_data("ascii_parse", data, len); //print_data("ascii_parse", data, len);
end = (const char *)(data + len); end = (const char *)(data + len);
for (p = (const char *)data ; p < end; p++) { for (p = (const char *)data ; p < end; p++) {
if (*p == '-') { if (ascii_state == ASCII_STATE_WORD) {
//printf("ascii_parse negative\n"); if (ascii_count == 0) {
if (ascii_count > 0) goto fail; if (*p == 'R') {
ascii_neg = 1; ascii_num = ASCII_STATE_RAW;
} else if (isdigit(*p)) { ascii_count = 1;
//printf("ascii_parse digit\n"); } else if (*p == 'C') {
ascii_num = ascii_num * 10 + *p - '0'; ascii_num = ASCII_STATE_CAL1;
ascii_count++; ascii_count = 1;
} else if (*p == ',') { }
//printf("ascii_parse comma, %d\n", ascii_num); } else if (ascii_count == 1) {
if (ascii_neg) ascii_num = -ascii_num; if (*p == 'a') {
if (ascii_num < -32768 && ascii_num > 32767) goto fail; ascii_count = 2;
if (ascii_raw_data_count >= 8) goto fail; } else {
ascii_raw_data[ascii_raw_data_count++] = ascii_num; ascii_num = 0;
ascii_num = 0; ascii_count = 0;
ascii_neg = 0; }
ascii_count = 0; } else if (ascii_count == 2) {
} else if (*p == 13) { if (*p == 'w' && ascii_num == ASCII_STATE_RAW) {
//printf("ascii_parse newline\n"); ascii_count = 3;
if (ascii_neg) ascii_num = -ascii_num; } else if (*p == 'l' && ascii_num == ASCII_STATE_CAL1) {
if (ascii_num < -32768 && ascii_num > 32767) goto fail; ascii_count = 3;
if (ascii_raw_data_count != 8) goto fail; } else {
ascii_raw_data[ascii_raw_data_count] = ascii_num; ascii_num = 0;
raw_data(ascii_raw_data); ascii_count = 0;
ret = 1; }
ascii_raw_data_count = 0; } else if (ascii_count == 3) {
ascii_num = 0; if (*p == ':' && ascii_num == ASCII_STATE_RAW) {
ascii_neg = 0; ascii_state = ASCII_STATE_RAW;
ascii_count = 0; ascii_raw_data_count = 0;
} else if (*p == 10) { ascii_num = 0;
} else { ascii_count = 0;
goto fail; } 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; return ret;
fail: fail:
printf("ascii FAIL\n");
ascii_state = ASCII_STATE_WORD;
ascii_raw_data_count = 0; ascii_raw_data_count = 0;
ascii_num = 0; ascii_num = 0;
ascii_neg = 0; ascii_neg = 0;