Use new calibration data format
This commit is contained in:
PaulStoffregen
parent
3e73181ee7
commit
b11220a178
3 changed files with 193 additions and 40 deletions
|
|
@ -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);
|
||||
|
|
|
|||
47
rawdata.c
47
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);
|
||||
}
|
||||
|
||||
|
|
|
|||
118
serialdata.c
118
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,6 +195,65 @@ 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 (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;
|
||||
|
|
@ -217,13 +283,65 @@ static int ascii_parse(const unsigned char *data, int len)
|
|||
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;
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue