rendered paste body#include <stdio.h>#include <fcntl.h>#include <string.h>#include <errno.h>#include <termios.h>#include <stdlib.h>#include <unistd.h>#define SER_DEVICE "/dev/cu.KeySerial1"#define SER_SPEED B115200#define NUM_NODES 8#define NUM_SENSORS 6#define PKT_LEN 16int cel_ser_setup();int cel_ser_shutdown();short * cel_ser_read();int check_offset(unsigned char buf[], int offset);int do_read(unsigned char buf[], int len);int cel_ser_findstart(unsigned char * buf);int convert_input(unsigned char * buf, int len);void print_packet(unsigned char buf[], int len);int fd = -1;struct termios term_before;short data[NUM_NODES * (NUM_SENSORS + 1)] = {0};int main(void) { printf("Setting up serial port\n"); if (cel_ser_setup()) printf("Setup serial port\n"); else printf("Failed to setup serial port\n"); //printf("Do something here\n"); cel_ser_read(); printf("got past first read\n"); /* while(1) { int i, j; short *blah; blah = cel_ser_read(); for(i=0;i < NUM_NODES; i++) { short * cur; cur = &blah[i * (NUM_SENSORS + 1)]; printf("mode: %d ", cur[0]); for(j=0; j < NUM_SENSORS; j++) { char * sensors[] = {"x", "y", "z", "yaw", "pitch", "roll"}; printf("%s: %d ", sensors[j], cur[0]); } printf("\n"); } printf("\n"); } */ if (cel_ser_shutdown() < 0) printf("Failed to setup serial port\n"); else printf("Shutdown serial port\n"); return 0;}int cel_ser_setup(){ static int done = 0; struct termios term; int ret; if(done) { fprintf(stderr, "cel_ser_setup: Setup already done.\n"); return 0; } // Open serial port fd = open(SER_DEVICE, O_RDWR | O_NOCTTY); // | O_NONBLOCK); if (fd < 0) { fprintf(stderr, "cel_ser_setup: Failed to open %s: %s\n", SER_DEVICE, strerror(errno)); return 0; } // Read in current serial port settings ret = tcgetattr(fd, &term); if (ret < 0) { fprintf(stderr, "cel_ser_setup: Failed get status of %s: %s\n", SER_DEVICE, strerror(errno)); cel_ser_shutdown(); return 0; } // Backup current serial port settings memcpy(&term_before, &term, sizeof(struct termios)); // Clear termios structure cfmakeraw(&term); // Set serial port speed ret = cfsetspeed(&term, SER_SPEED); if (ret < 0) { fprintf(stderr, "cel_ser_setup: Failed to speed of %s to %s: %s\n", SER_DEVICE, SER_SPEED, strerror(errno)); cel_ser_shutdown(); return 0; } // Set data size term.c_cflag &= ~CSIZE; term.c_cflag |= CS8; // Set no parity term.c_cflag &= ~PARENB; // Set stop bits term.c_cflag &= ~CSTOPB; //Set no software flow control term.c_cflag &= ~(IXON | IXOFF | IXANY); //Set no hardware flow control term.c_cflag &= ~CRTSCTS; //Set raw mode input mode term.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); //Set raw output mode term.c_oflag &= ~OPOST; // Write out new serial port settings ret = tcsetattr(fd, TCSANOW, &term); if (ret < 0) { fprintf(stderr, "cel_ser_setup: Failed to write out new settings to %s: %s\n", SER_DEVICE, strerror(errno)); cel_ser_shutdown(); return 0; } done = 1; printf("Setup of %s done\n", SER_DEVICE); return 1;}int cel_ser_shutdown(){ int ret; if(fd < 0) return 1; // Write out old serial port settings ret = tcsetattr(fd, TCSANOW, &term_before); if (ret < 0) { fprintf(stderr, "cel_ser_shutdown: Failed to write out old settings to %s: %s\n", SER_DEVICE, strerror(errno)); } // Close serial port ret = close(fd); if (ret < 0) { fprintf(stderr, "cel_ser_shutdown: Error closing device %s: %s\n", SER_DEVICE, strerror(errno)); return 0; } return 1;}short * cel_ser_read(){ int started = 0; int done = 0; int i, j, offset = -1; short val = 0; unsigned char buf[PKT_LEN * 2]; unsigned char start_bytes[] = {'\r', '\n'}; // Reset the data memset(data, 0xFF, sizeof(data)); for(i=0; i < NUM_NODES; i++) data[i * NUM_SENSORS] = i + 1; while(!done) { if(!started) { offset = cel_ser_findstart(buf); printf("findstart returned\n"); if(offset < 0) return NULL; else started = 1; } for(i=0; i < NUM_NODES; i++) { if(i != 0 || offset < 0) { if(!do_read(buf, PKT_LEN)) return NULL; } if(!check_offset(buf, 0)) { fprintf(stderr, "cel_ser_read: Packet stream corrupt, resyncing\n"); started = 0; continue; } else { short * cur; cur = &data[i * (NUM_SENSORS + 1)]; if(i != (buf[0] - '0')) { for(j=0; j< NUM_SENSORS; j++) cur[j] = -1; continue; } for(j=0; j< NUM_SENSORS; j++) //Store the values, masking off the top 4 bits cur[j] = buf[2 + (j * 2)] & 4096; } } } return data;}int check_offset(unsigned char buf[], int offset) { unsigned char *cur; //int i; cur = &buf[offset]; if(cur[0] < '1' || cur[0] > '8' || cur[1] != ':' || cur[14] != '\n' || cur[15] != '\r') return 0; /* for(i=0; i < NUM_SENSORS; i++) { if((cur[2 + (i * 2)] >> 4) != i + 2) return 0; } */ return 1; }int do_read(unsigned char * buf, int len) { int ret; ret = read(fd, buf, len); switch(ret) { case 0: fprintf(stderr, "cel_ser_read: Read EOF from device %s, shutting down\n", SER_DEVICE); cel_ser_shutdown(); return 0; break; case -1: fprintf(stderr, "cel_ser_read: Error reading from device %s, shutting down: %s\n", SER_DEVICE, strerror(errno)); cel_ser_shutdown(); return 0; break; default: if(ret == len) return 1; break; fprintf(stderr, "cel_ser_read: Failed to read requested number of bytes from device %s\n", SER_DEVICE); return 0; break; }}int cel_ser_findstart(unsigned char * buf){ int i, offset = -1; while(1) { printf("."); // Read in 2 packets worth of data so we can find the delimiter if(!do_read(buf, PKT_LEN * 2)) return -1; print_packet(buf, PKT_LEN * 2); // Search buffer for start delimiter for(i=0; i < PKT_LEN + 1; i++) { if(check_offset(buf, i)) { offset = i; break; } } // Copy first found packet to buffer if(offset >= 0) { memmove(buf, &buf[offset], PKT_LEN); return offset; } else continue; } return 1;}int convert_input(unsigned char * buf, int len){ int i; int val = 0; for(i=0; i < len; i++) { val |= buf[i] << (len - i); } return val; }void print_packet(unsigned char * buf, int len) { int i; for(i=0; i < len; i++) { /*if(buf[i] == '\r') printf(" \\r,"); else if(buf[i] == '\n') printf(" \\n,"); else if(isalnum(buf[i]) || ispunct(buf[i])) printf("%2c,", buf[i]); else*/ printf(" %02x,", buf[i]); } printf("\n"); fflush(stdout);}