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

58
serialdata.c
View file

@ -1,5 +1,44 @@
#include "imuread.h"
#ifndef MOTIONCAL_DEBUG
#define MOTIONCAL_DEBUG 0
#endif
#ifndef MOTIONCAL_DEBUG_BYTES
#define MOTIONCAL_DEBUG_BYTES 0
#endif
#if MOTIONCAL_DEBUG
#define DEBUG_PRINTF(...) fprintf(stderr, __VA_ARGS__)
#else
#define DEBUG_PRINTF(...) do { } while (0)
#endif
static void debug_ascii_preview(const unsigned char *data, int len)
{
#if MOTIONCAL_DEBUG_BYTES
int i;
fprintf(stderr, "MotionCal read %d bytes: ", len);
for (i = 0; i < len && i < 96; i++) {
unsigned char c = data[i];
if (c == '\r') {
fprintf(stderr, "\\r");
} else if (c == '\n') {
fprintf(stderr, "\\n");
} else if (isprint(c)) {
fputc(c, stderr);
} else {
fprintf(stderr, "\\x%02X", c);
}
}
if (len > 96) fprintf(stderr, "...");
fprintf(stderr, "\n");
#else
(void)data;
(void)len;
#endif
}
void print_data(const char *name, const unsigned char *data, int len)
{
@ -265,7 +304,7 @@ static int ascii_parse(const unsigned char *data, int len)
} 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_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;
@ -274,10 +313,14 @@ static int ascii_parse(const unsigned char *data, int len)
} 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_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);
DEBUG_PRINTF("MotionCal Raw parsed: %d,%d,%d,%d,%d,%d,%d,%d,%d\n",
ascii_raw_data[0], ascii_raw_data[1], ascii_raw_data[2],
ascii_raw_data[3], ascii_raw_data[4], ascii_raw_data[5],
ascii_raw_data[6], ascii_raw_data[7], ascii_raw_data[8]);
ret = 1;
ascii_raw_data_count = 0;
ascii_num = 0;
@ -340,7 +383,7 @@ static int ascii_parse(const unsigned char *data, int len)
}
return ret;
fail:
//printf("ascii FAIL\n");
DEBUG_PRINTF("MotionCal ascii parser reset\n");
ascii_state = ASCII_STATE_WORD;
ascii_raw_data_count = 0;
ascii_num = 0;
@ -375,9 +418,13 @@ int open_port(const char *name)
int r;
portfd = open(name, O_RDWR | O_NONBLOCK);
if (portfd < 0) return 0;
if (portfd < 0) {
DEBUG_PRINTF("MotionCal open failed for %s: %s\n", name, strerror(errno));
return 0;
}
r = tcgetattr(portfd, &termsettings);
if (r < 0) {
DEBUG_PRINTF("MotionCal tcgetattr failed for %s: %s\n", name, strerror(errno));
close_port();
return 0;
}
@ -385,9 +432,11 @@ int open_port(const char *name)
cfsetspeed(&termsettings, B115200);
r = tcsetattr(portfd, TCSANOW, &termsettings);
if (r < 0) {
DEBUG_PRINTF("MotionCal tcsetattr failed for %s: %s\n", name, strerror(errno));
close_port();
return 0;
}
DEBUG_PRINTF("MotionCal opened %s at 115200\n", name);
return 1;
}
@ -401,6 +450,7 @@ int read_serial_data(void)
while (1) {
n = read(portfd, buf, sizeof(buf));
if (n > 0 && n <= sizeof(buf)) {
debug_ascii_preview(buf, n);
newdata(buf, n);
nodata_count = 0;
return n;