Separate communication & graphics code
This commit is contained in:
PaulStoffregen
parent
2a0251ff6a
commit
22ef4bdfbe
4 changed files with 325 additions and 306 deletions
240
serialdata.c
Normal file
240
serialdata.c
Normal file
|
|
@ -0,0 +1,240 @@
|
|||
#include "imuread.h"
|
||||
|
||||
static int portfd=-1;
|
||||
|
||||
void print_data(const char *name, const unsigned char *data, int len)
|
||||
{
|
||||
int i;
|
||||
|
||||
printf("%s (%2d bytes):", name, len);
|
||||
for (i=0; i < len; i++) {
|
||||
printf(" %02X", data[i]);
|
||||
}
|
||||
printf("\n");
|
||||
}
|
||||
|
||||
void packet_primary_data(const unsigned char *data)
|
||||
{
|
||||
current_position.x = (float)((int16_t)((data[13] << 8) | data[12])) / 10.0f;
|
||||
current_position.y = (float)((int16_t)((data[15] << 8) | data[14])) / 10.0f;
|
||||
current_position.z = (float)((int16_t)((data[17] << 8) | data[16])) / 10.0f;
|
||||
current_orientation.w = (float)((int16_t)((data[25] << 8) | data[24])) / 30000.0f;
|
||||
current_orientation.x = (float)((int16_t)((data[27] << 8) | data[26])) / 30000.0f;
|
||||
current_orientation.y = (float)((int16_t)((data[29] << 8) | data[28])) / 30000.0f;
|
||||
current_orientation.z = (float)((int16_t)((data[31] << 8) | data[30])) / 30000.0f;
|
||||
#if 0
|
||||
printf("mag data, %5.2f %5.2f %5.2f\n",
|
||||
current_position.x,
|
||||
current_position.y,
|
||||
current_position.z
|
||||
);
|
||||
#endif
|
||||
#if 0
|
||||
printf("orientation: %5.3f %5.3f %5.3f %5.3f\n",
|
||||
current_orientation.w,
|
||||
current_orientation.x,
|
||||
current_orientation.y,
|
||||
current_orientation.z
|
||||
);
|
||||
#endif
|
||||
}
|
||||
|
||||
void packet_magnetic_cal(const unsigned char *data)
|
||||
{
|
||||
int16_t id, x, y, z;
|
||||
magdata_t *cal;
|
||||
float newx, newy, newz;
|
||||
|
||||
id = (data[7] << 8) | data[6];
|
||||
x = (data[9] << 8) | data[8];
|
||||
y = (data[11] << 8) | data[10];
|
||||
z = (data[13] << 8) | data[12];
|
||||
|
||||
if (id == 1) {
|
||||
cal = &hard_iron;
|
||||
cal->x = (float)x / 10.0f;
|
||||
cal->y = (float)y / 10.0f;
|
||||
cal->z = (float)z / 10.0f;
|
||||
cal->valid = 1;
|
||||
} else if (id >= 10 && id < MAGBUFFSIZE+10) {
|
||||
newx = (float)x / 10.0f;
|
||||
newy = (float)y / 10.0f;
|
||||
newz = (float)z / 10.0f;
|
||||
cal = &caldata[id - 10];
|
||||
if (!cal->valid || cal->x != newx || cal->y != newy || cal->z != newz) {
|
||||
cal->x = newx;
|
||||
cal->y = newy;
|
||||
cal->z = newz;
|
||||
cal->valid = 1;
|
||||
printf("mag cal, id=%3d: %5d %5d %5d\n", id, x, y, z);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void packet(const unsigned char *data, int len)
|
||||
{
|
||||
if (len <= 0) return;
|
||||
//print_data("packet", data, len);
|
||||
|
||||
if (data[0] == 1 && len == 34) {
|
||||
packet_primary_data(data);
|
||||
} else if (data[0] == 6 && len == 14) {
|
||||
packet_magnetic_cal(data);
|
||||
}
|
||||
// TODO: actually do something with the arriving data...
|
||||
}
|
||||
|
||||
void packet_encoded(const unsigned char *data, int len)
|
||||
{
|
||||
const unsigned char *p;
|
||||
unsigned char buf[256];
|
||||
int buflen=0, copylen;
|
||||
|
||||
//printf("packet_encoded, len = %d\n", len);
|
||||
p = memchr(data, 0x7D, len);
|
||||
if (p == NULL) {
|
||||
packet(data, len);
|
||||
} else {
|
||||
//printf("** decoding necessary\n");
|
||||
while (1) {
|
||||
copylen = p - data;
|
||||
if (copylen > 0) {
|
||||
//printf(" copylen = %d\n", copylen);
|
||||
// TODO: overflow check
|
||||
memcpy(buf+buflen, data, copylen);
|
||||
buflen += copylen;
|
||||
data += copylen;
|
||||
len -= copylen;
|
||||
}
|
||||
// TODO: overflow check
|
||||
buf[buflen++] = (p[1] == 0x5E) ? 0x7E : 0x7D;
|
||||
data += 2;
|
||||
len -= 2;
|
||||
if (len <= 0) break;
|
||||
p = memchr(data, 0x7D, len);
|
||||
if (p == NULL) {
|
||||
// TODO: overflow check
|
||||
memcpy(buf+buflen, data, len);
|
||||
buflen += len;
|
||||
break;
|
||||
}
|
||||
}
|
||||
//printf("** decoded to %d\n", buflen);
|
||||
packet(buf, buflen);
|
||||
}
|
||||
}
|
||||
|
||||
void newdata(const unsigned char *data, int len)
|
||||
{
|
||||
static unsigned char packetbuf[256];
|
||||
static unsigned int packetlen=0;
|
||||
const unsigned char *p;
|
||||
int copylen;
|
||||
|
||||
//print_data("newdata", data, len);
|
||||
while (len > 0) {
|
||||
p = memchr(data, 0x7E, len);
|
||||
if (p == NULL) {
|
||||
// TODO: overflow check
|
||||
memcpy(packetbuf+packetlen, data, len);
|
||||
packetlen += len;
|
||||
len = 0;
|
||||
} else if (p > data) {
|
||||
copylen = p - data;
|
||||
// TODO: overflow check
|
||||
memcpy(packetbuf+packetlen, data, copylen);
|
||||
packet_encoded(packetbuf, packetlen+copylen);
|
||||
packetlen = 0;
|
||||
data += copylen + 1;
|
||||
len -= copylen + 1;
|
||||
} else {
|
||||
if (packetlen > 0) {
|
||||
packet_encoded(packetbuf, packetlen);
|
||||
packetlen = 0;
|
||||
}
|
||||
data++;
|
||||
len--;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int open_port(const char *name)
|
||||
{
|
||||
struct termios termsettings;
|
||||
int r;
|
||||
|
||||
portfd = open(name, O_RDWR | O_NONBLOCK);
|
||||
if (portfd < 0) die("Unable to open %s\n", name);
|
||||
|
||||
r = tcgetattr(portfd, &termsettings);
|
||||
if (r < 0) die("Unable to read terminal settings from %s\n", name);
|
||||
cfmakeraw(&termsettings);
|
||||
cfsetspeed(&termsettings, B115200);
|
||||
r = tcsetattr(portfd, TCSANOW, &termsettings);
|
||||
if (r < 0) die("Unable to program terminal settings on %s\n", name);
|
||||
return 0;
|
||||
}
|
||||
|
||||
void read_serial_data(void)
|
||||
{
|
||||
unsigned char buf[256];
|
||||
static int nodata_count=0;
|
||||
int n;
|
||||
|
||||
while (portfd >= 0) {
|
||||
n = read(portfd, buf, sizeof(buf));
|
||||
if (n > 0 && n <= sizeof(buf)) {
|
||||
//printf("read %d bytes\n", r);
|
||||
newdata(buf, n);
|
||||
nodata_count = 0;
|
||||
} else if (n == 0) {
|
||||
printf("read no data\n");
|
||||
if (++nodata_count > 6) {
|
||||
close_port();
|
||||
die("No data from %s\n", PORT);
|
||||
nodata_count = 0;
|
||||
}
|
||||
break;
|
||||
} else {
|
||||
n = errno;
|
||||
if (n == EAGAIN) {
|
||||
break;
|
||||
} else if (n == EAGAIN) {
|
||||
} else {
|
||||
printf("read error, n=%d\n", n);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void close_port(void)
|
||||
{
|
||||
if (portfd >= 0) {
|
||||
close(portfd);
|
||||
portfd = -1;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void die(const char *format, ...)
|
||||
{
|
||||
va_list args;
|
||||
va_start(args, format);
|
||||
vfprintf(stderr, format, args);
|
||||
exit(1);
|
||||
}
|
||||
|
||||
void *malloc_or_die(size_t size)
|
||||
{
|
||||
void *p;
|
||||
|
||||
p = malloc(size);
|
||||
if (!p) die("unable to allocate memory, %d bytes\n", (int)size);
|
||||
return p;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
Loading…
Add table
Add a link
Reference in a new issue